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

pass AnyExecutable objects as reference to avoid memory allocation (#463)

Browse source 
* pass AnyExecutable objects as reference to avoid memory allocation

* remove style change
This commit is contained in:
William Woodall 2018-04-17 21:54:42 -05:00 • committed by GitHub
parent 360f1b9425
commit 1610fc3973
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 88 additions and 94 deletions
Download patch file Download diff file Expand all files Collapse all files

2
rclcpp/include/rclcpp/any_executable.hpp
View file

@ -33,8 +33,6 @@ namespace executor
struct AnyExecutable struct AnyExecutable
{ {
RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable)
RCLCPP_PUBLIC RCLCPP_PUBLIC
AnyExecutable(); AnyExecutable();

14
rclcpp/include/rclcpp/executor.hpp
View file

@ -287,7 +287,7 @@ protected:
*/ */
RCLCPP_PUBLIC RCLCPP_PUBLIC
void void
execute_any_executable(AnyExecutable::SharedPtr any_exec); execute_any_executable(AnyExecutable & any_exec);
RCLCPP_PUBLIC RCLCPP_PUBLIC
static void static void
@ -325,15 +325,17 @@ protected:
RCLCPP_PUBLIC RCLCPP_PUBLIC
void void
get_next_timer(AnyExecutable::SharedPtr any_exec); get_next_timer(AnyExecutable & any_exec);
RCLCPP_PUBLIC RCLCPP_PUBLIC
AnyExecutable::SharedPtr bool
get_next_ready_executable(); get_next_ready_executable(AnyExecutable & any_executable);
RCLCPP_PUBLIC RCLCPP_PUBLIC
AnyExecutable::SharedPtr bool
get_next_executable(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); get_next_executable(
AnyExecutable & any_executable,
std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
/// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
std::atomic_bool spinning; std::atomic_bool spinning;

10
rclcpp/include/rclcpp/memory_strategy.hpp
View file

@ -55,27 +55,23 @@ public:
virtual void clear_handles() = 0; virtual void clear_handles() = 0;
virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0; virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
/// Provide a newly initialized AnyExecutable object.
// \return Shared pointer to the fresh executable.
virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
virtual void virtual void
get_next_subscription( get_next_subscription(
rclcpp::executor::AnyExecutable::SharedPtr any_exec, rclcpp::executor::AnyExecutable & any_exec,
const WeakNodeVector & weak_nodes) = 0; const WeakNodeVector & weak_nodes) = 0;
virtual void virtual void
get_next_service( get_next_service(
rclcpp::executor::AnyExecutable::SharedPtr any_exec, rclcpp::executor::AnyExecutable & any_exec,
const WeakNodeVector & weak_nodes) = 0; const WeakNodeVector & weak_nodes) = 0;
virtual void virtual void
get_next_client( get_next_client(
rclcpp::executor::AnyExecutable::SharedPtr any_exec, rclcpp::executor::AnyExecutable & any_exec,
const WeakNodeVector & weak_nodes) = 0; const WeakNodeVector & weak_nodes) = 0;
virtual rcl_allocator_t virtual rcl_allocator_t

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

@ -48,21 +48,16 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
public: public:
RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>) RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>)
using ExecAllocTraits = allocator::AllocRebind<executor::AnyExecutable, Alloc>;
using ExecAlloc = typename ExecAllocTraits::allocator_type;
using ExecDeleter = allocator::Deleter<ExecAlloc, executor::AnyExecutable>;
using VoidAllocTraits = typename allocator::AllocRebind<void *, Alloc>; using VoidAllocTraits = typename allocator::AllocRebind<void *, Alloc>;
using VoidAlloc = typename VoidAllocTraits::allocator_type; using VoidAlloc = typename VoidAllocTraits::allocator_type;
explicit AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator) explicit AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
{ {
executable_allocator_ = std::make_shared<ExecAlloc>(*allocator.get());
allocator_ = std::make_shared<VoidAlloc>(*allocator.get()); allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
} }
AllocatorMemoryStrategy() AllocatorMemoryStrategy()
{ {
executable_allocator_ = std::make_shared<ExecAlloc>();
allocator_ = std::make_shared<VoidAlloc>(); allocator_ = std::make_shared<VoidAlloc>();
} }
@ -235,16 +230,9 @@ public:
return true; return true;
} }
/// Provide a newly initialized AnyExecutable object.
// \return Shared pointer to the fresh executable.
executor::AnyExecutable::SharedPtr instantiate_next_executable()
{
return std::allocate_shared<executor::AnyExecutable>(*executable_allocator_.get());
}
virtual void virtual void
get_next_subscription( get_next_subscription(
executor::AnyExecutable::SharedPtr any_exec, executor::AnyExecutable & any_exec,
const WeakNodeVector & weak_nodes) const WeakNodeVector & weak_nodes)
{ {
auto it = subscription_handles_.begin(); auto it = subscription_handles_.begin();
@ -272,12 +260,12 @@ public:
} }
// Otherwise it is safe to set and return the any_exec // Otherwise it is safe to set and return the any_exec
if (is_intra_process) { if (is_intra_process) {
any_exec->subscription_intra_process = subscription; any_exec.subscription_intra_process = subscription;
} else { } else {
any_exec->subscription = subscription; any_exec.subscription = subscription;
} }
any_exec->callback_group = group; any_exec.callback_group = group;
any_exec->node_base = get_node_by_group(group, weak_nodes); any_exec.node_base = get_node_by_group(group, weak_nodes);
subscription_handles_.erase(it); subscription_handles_.erase(it);
return; return;
} }
@ -288,7 +276,7 @@ public:
virtual void virtual void
get_next_service( get_next_service(
executor::AnyExecutable::SharedPtr any_exec, executor::AnyExecutable & any_exec,
const WeakNodeVector & weak_nodes) const WeakNodeVector & weak_nodes)
{ {
auto it = service_handles_.begin(); auto it = service_handles_.begin();
@ -310,9 +298,9 @@ public:
continue; continue;
} }
// Otherwise it is safe to set and return the any_exec // Otherwise it is safe to set and return the any_exec
any_exec->service = service; any_exec.service = service;
any_exec->callback_group = group; any_exec.callback_group = group;
any_exec->node_base = get_node_by_group(group, weak_nodes); any_exec.node_base = get_node_by_group(group, weak_nodes);
service_handles_.erase(it); service_handles_.erase(it);
return; return;
} }
@ -322,7 +310,7 @@ public:
} }
virtual void virtual void
get_next_client(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes) get_next_client(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes)
{ {
auto it = client_handles_.begin(); auto it = client_handles_.begin();
while (it != client_handles_.end()) { while (it != client_handles_.end()) {
@ -343,9 +331,9 @@ public:
continue; continue;
} }
// Otherwise it is safe to set and return the any_exec // Otherwise it is safe to set and return the any_exec
any_exec->client = client; any_exec.client = client;
any_exec->callback_group = group; any_exec.callback_group = group;
any_exec->node_base = get_node_by_group(group, weak_nodes); any_exec.node_base = get_node_by_group(group, weak_nodes);
client_handles_.erase(it); client_handles_.erase(it);
return; return;
} }
@ -396,7 +384,6 @@ private:
VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_; VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_;
VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_; VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
std::shared_ptr<ExecAlloc> executable_allocator_;
std::shared_ptr<VoidAlloc> allocator_; std::shared_ptr<VoidAlloc> allocator_;
}; };

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

@ -206,9 +206,13 @@ Executor::spin_some()
throw std::runtime_error("spin_some() called while already spinning"); throw std::runtime_error("spin_some() called while already spinning");
} }
RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
AnyExecutable::SharedPtr any_exec; while (spinning.load()) {
while ((any_exec = get_next_executable(std::chrono::milliseconds::zero())) && spinning.load()) { AnyExecutable any_exec;
execute_any_executable(any_exec); if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) {
execute_any_executable(any_exec);
} else {
break;
}
} }
} }
@ -219,8 +223,8 @@ Executor::spin_once(std::chrono::nanoseconds timeout)
throw std::runtime_error("spin_once() called while already spinning"); throw std::runtime_error("spin_once() called while already spinning");
} }
RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
auto any_exec = get_next_executable(timeout); AnyExecutable any_exec;
if (any_exec) { if (get_next_executable(any_exec, timeout)) {
execute_any_executable(any_exec); execute_any_executable(any_exec);
} }
} }
@ -244,28 +248,28 @@ Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr
} }
void void
Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec) Executor::execute_any_executable(AnyExecutable & any_exec)
{ {
if (!any_exec || !spinning.load()) { if (!spinning.load()) {
return; return;
} }
if (any_exec->timer) { if (any_exec.timer) {
execute_timer(any_exec->timer); execute_timer(any_exec.timer);
} }
if (any_exec->subscription) { if (any_exec.subscription) {
execute_subscription(any_exec->subscription); execute_subscription(any_exec.subscription);
} }
if (any_exec->subscription_intra_process) { if (any_exec.subscription_intra_process) {
execute_intra_process_subscription(any_exec->subscription_intra_process); execute_intra_process_subscription(any_exec.subscription_intra_process);
} }
if (any_exec->service) { if (any_exec.service) {
execute_service(any_exec->service); execute_service(any_exec.service);
} }
if (any_exec->client) { if (any_exec.client) {
execute_client(any_exec->client); execute_client(any_exec.client);
} }
// Reset the callback_group, regardless of type // Reset the callback_group, regardless of type
any_exec->callback_group->can_be_taken_from().store(true); any_exec.callback_group->can_be_taken_from().store(true);
// Wake the wait, because it may need to be recalculated or work that // Wake the wait, because it may need to be recalculated or work that
// was previously blocked is now available. // was previously blocked is now available.
if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
@ -508,7 +512,7 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
} }
void void
Executor::get_next_timer(AnyExecutable::SharedPtr any_exec) Executor::get_next_timer(AnyExecutable & any_exec)
{ {
for (auto & weak_node : weak_nodes_) { for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock(); auto node = weak_node.lock();
@ -523,8 +527,8 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
for (auto & timer_ref : group->get_timer_ptrs()) { for (auto & timer_ref : group->get_timer_ptrs()) {
auto timer = timer_ref.lock(); auto timer = timer_ref.lock();
if (timer && timer->is_ready()) { if (timer && timer->is_ready()) {
any_exec->timer = timer; any_exec.timer = timer;
any_exec->callback_group = group; any_exec.callback_group = group;
node = get_node_by_group(group); node = get_node_by_group(group);
return; return;
} }
@ -533,67 +537,69 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
} }
} }
AnyExecutable::SharedPtr bool
Executor::get_next_ready_executable() Executor::get_next_ready_executable(AnyExecutable & any_executable)
{ {
auto any_exec = memory_strategy_->instantiate_next_executable();
// Check the timers to see if there are any that are ready, if so return // Check the timers to see if there are any that are ready, if so return
get_next_timer(any_exec); get_next_timer(any_executable);
if (any_exec->timer) { if (any_executable.timer) {
return any_exec; return true;
} }
// Check the subscriptions to see if there are any that are ready // Check the subscriptions to see if there are any that are ready
memory_strategy_->get_next_subscription(any_exec, weak_nodes_); memory_strategy_->get_next_subscription(any_executable, weak_nodes_);
if (any_exec->subscription || any_exec->subscription_intra_process) { if (any_executable.subscription || any_executable.subscription_intra_process) {
return any_exec; return true;
} }
// Check the services to see if there are any that are ready // Check the services to see if there are any that are ready
memory_strategy_->get_next_service(any_exec, weak_nodes_); memory_strategy_->get_next_service(any_executable, weak_nodes_);
if (any_exec->service) { if (any_executable.service) {
return any_exec; return true;
} }
// Check the clients to see if there are any that are ready // Check the clients to see if there are any that are ready
memory_strategy_->get_next_client(any_exec, weak_nodes_); memory_strategy_->get_next_client(any_executable, weak_nodes_);
if (any_exec->client) { if (any_executable.client) {
return any_exec; return true;
} }
// If there is no ready executable, return a null ptr // If there is no ready executable, return a null ptr
return nullptr; return false;
} }
AnyExecutable::SharedPtr bool
Executor::get_next_executable(std::chrono::nanoseconds timeout) Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout)
{ {
bool success = false;
// Check to see if there are any subscriptions or timers needing service // Check to see if there are any subscriptions or timers needing service
// TODO(wjwwood): improve run to run efficiency of this function // TODO(wjwwood): improve run to run efficiency of this function
auto any_exec = get_next_ready_executable(); success = get_next_ready_executable(any_executable);
// If there are none // If there are none
if (!any_exec) { if (!success) {
// Wait for subscriptions or timers to work on // Wait for subscriptions or timers to work on
wait_for_work(timeout); wait_for_work(timeout);
if (!spinning.load()) { if (!spinning.load()) {
return nullptr; return false;
} }
// Try again // Try again
any_exec = get_next_ready_executable(); success = get_next_ready_executable(any_executable);
} }
// At this point any_exec should be valid with either a valid subscription // At this point any_exec should be valid with either a valid subscription
// or a valid timer, or it should be a null shared_ptr // or a valid timer, or it should be a null shared_ptr
if (any_exec) { if (success) {
// If it is valid, check to see if the group is mutually exclusive or // If it is valid, check to see if the group is mutually exclusive or
// not, then mark it accordingly // not, then mark it accordingly
if (any_exec->callback_group && any_exec->callback_group->type() == \ using callback_group::CallbackGroupType;
callback_group::CallbackGroupType::MutuallyExclusive) if (
any_executable.callback_group &&
any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive)
{ {
// It should not have been taken otherwise // It should not have been taken otherwise
assert(any_exec->callback_group->can_be_taken_from().load()); assert(any_executable.callback_group->can_be_taken_from().load());
// Set to false to indicate something is being run from this group // Set to false to indicate something is being run from this group
// This is reset to true either when the any_exec is executed or when the // This is reset to true either when the any_exec is executed or when the
// any_exec is destructued // any_exec is destructued
any_exec->callback_group->can_be_taken_from().store(false); any_executable.callback_group->can_be_taken_from().store(false);
} }
} }
return any_exec; return success;
} }
std::ostream & std::ostream &

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

@ -69,13 +69,15 @@ void
MultiThreadedExecutor::run(size_t) MultiThreadedExecutor::run(size_t)
{ {
while (rclcpp::ok() && spinning.load()) { while (rclcpp::ok() && spinning.load()) {
executor::AnyExecutable::SharedPtr any_exec; executor::AnyExecutable any_exec;
{ {
std::lock_guard<std::mutex> wait_lock(wait_mutex_); std::lock_guard<std::mutex> wait_lock(wait_mutex_);
if (!rclcpp::ok() || !spinning.load()) { if (!rclcpp::ok() || !spinning.load()) {
return; return;
} }
any_exec = get_next_executable(); if (!get_next_executable(any_exec)) {
continue;
}
} }
execute_any_executable(any_exec); execute_any_executable(any_exec);
} }

7
rclcpp/src/rclcpp/executors/single_threaded_executor.cpp
View file

@ -13,6 +13,7 @@
// limitations under the License. // limitations under the License.
#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp"
#include "rclcpp/any_executable.hpp"
#include "rclcpp/scope_exit.hpp" #include "rclcpp/scope_exit.hpp"
using rclcpp::executors::SingleThreadedExecutor; using rclcpp::executors::SingleThreadedExecutor;
@ -30,7 +31,9 @@ SingleThreadedExecutor::spin()
} }
RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
while (rclcpp::ok() && spinning.load()) { while (rclcpp::ok() && spinning.load()) {
auto any_exec = get_next_executable(); rclcpp::executor::AnyExecutable any_executable;
execute_any_executable(any_exec); if (get_next_executable(any_executable)) {
execute_any_executable(any_executable);
}
} }
} }