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

Merge pull request #81 from ros2/realtime_cleanup

Browse source 
Configure StaticMemoryStrategy, pointer/reference correctness
This commit is contained in:
Jackie Kay 2015-08-10 15:56:20 -07:00
commit e57b049aeb
3 changed files with 74 additions and 58 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -293,7 +293,7 @@ protected:
subscriber_handles.subscriber_count = number_of_subscriptions;
// TODO(wjwwood): Avoid redundant malloc's
subscriber_handles.subscribers =
memory_strategy_->borrow_handles(HandleType::subscriber_handle, number_of_subscriptions);
memory_strategy_->borrow_handles(HandleType::subscription_handle, number_of_subscriptions);
if (subscriber_handles.subscribers == NULL) {
// TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for subscriber pointers.");
@ -397,7 +397,7 @@ protected:
// If ctrl-c guard condition, return directly
if (guard_condition_handles.guard_conditions[0] != 0) {
// Make sure to free or clean memory
memory_strategy_->return_handles(HandleType::subscriber_handle,
memory_strategy_->return_handles(HandleType::subscription_handle,
subscriber_handles.subscribers);
memory_strategy_->return_handles(HandleType::service_handle,
service_handles.services);
@ -430,7 +430,7 @@ protected:
}
}
memory_strategy_->return_handles(HandleType::subscriber_handle,
memory_strategy_->return_handles(HandleType::subscription_handle,
subscriber_handles.subscribers);
memory_strategy_->return_handles(HandleType::service_handle,
service_handles.services);
@ -446,17 +446,17 @@ protected:
rclcpp::subscription::SubscriptionBase::SharedPtr
get_subscription_by_handle(void * subscriber_handle)
{
for (auto weak_node : weak_nodes_) {
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto weak_group : node->callback_groups_) {
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto weak_subscription : group->subscription_ptrs_) {
for (auto & weak_subscription : group->subscription_ptrs_) {
auto subscription = weak_subscription.lock();
if (subscription && subscription->subscription_handle_->data == subscriber_handle) {
return subscription;
@ -470,17 +470,17 @@ protected:
rclcpp::service::ServiceBase::SharedPtr
get_service_by_handle(void * service_handle)
{
for (auto weak_node : weak_nodes_) {
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto weak_group : node->callback_groups_) {
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto service : group->service_ptrs_) {
for (auto & service : group->service_ptrs_) {
if (service->service_handle_->data == service_handle) {
return service;
}
@ -493,17 +493,17 @@ protected:
rclcpp::client::ClientBase::SharedPtr
get_client_by_handle(void * client_handle)
{
for (auto weak_node : weak_nodes_) {
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto weak_group : node->callback_groups_) {
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto client : group->client_ptrs_) {
for (auto & client : group->client_ptrs_) {
if (client->client_handle_->data == client_handle) {
return client;
}
@ -526,9 +526,6 @@ protected:
}
for (auto & weak_group : node->callback_groups_) {
auto callback_group = weak_group.lock();
if (!callback_group) {
continue;
}
if (callback_group == group) {
return node;
}
@ -548,6 +545,9 @@ protected:
}
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & weak_timer : group->timer_ptrs_) {
auto t = weak_timer.lock();
if (t == timer) {
@ -627,6 +627,9 @@ protected:
}
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & weak_sub : group->subscription_ptrs_) {
auto sub = weak_sub.lock();
if (sub == subscription) {
@ -682,6 +685,9 @@ protected:
}
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & serv : group->service_ptrs_) {
if (serv == service) {
return group;
@ -736,6 +742,9 @@ protected:
}
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & cli : group->client_ptrs_) {
if (cli == client) {
return group;

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

@ -20,7 +20,7 @@ namespace rclcpp
{
// TODO move HandleType somewhere where it makes sense
enum class HandleType {subscriber_handle, service_handle, client_handle, guard_condition_handle};
enum class HandleType {subscription_handle, service_handle, client_handle, guard_condition_handle};
namespace executor
{

91
rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp
View file

@ -27,24 +27,46 @@ namespace memory_strategies
namespace static_memory_strategy
{
struct ObjectPoolBounds
{
public:
size_t max_subscriptions_;
size_t max_services_;
size_t max_clients_;
size_t max_executables_;
size_t pool_size_;
ObjectPoolBounds(size_t subs = 10, size_t services = 10, size_t clients = 10,
size_t executables = 1, size_t pool = 1024)
: max_subscriptions_(subs), max_services_(services), max_clients_(clients), max_executables_(
executables), pool_size_(pool)
{}
};
class StaticMemoryStrategy : public memory_strategy::MemoryStrategy
{
public:
StaticMemoryStrategy()
StaticMemoryStrategy(ObjectPoolBounds bounds = ObjectPoolBounds())
: bounds_(bounds)
{
memset(memory_pool_, 0, pool_size_);
memset(subscriber_pool_, 0, max_subscribers_);
memset(service_pool_, 0, max_services_);
memset(client_pool_, 0, max_clients_);
memset(guard_condition_pool_, 0, max_guard_conditions_);
memory_pool_ = static_cast<void **>(malloc(bounds_.pool_size_));
subscription_pool_ = static_cast<void **>(malloc(bounds_.max_subscriptions_));
service_pool_ = static_cast<void **>(malloc(bounds_.max_services_));
client_pool_ = static_cast<void **>(malloc(bounds_.max_clients_));
executable_pool_ = static_cast<executor::AnyExecutable *>(malloc(bounds_.max_executables_));
memset(memory_pool_, 0, bounds_.pool_size_);
memset(subscription_pool_, 0, bounds_.max_subscriptions_);
memset(service_pool_, 0, bounds_.max_services_);
memset(client_pool_, 0, bounds_.max_clients_);
memset(executable_pool_, 0, bounds_.max_executables_);
pool_seq_ = 0;
exec_seq_ = 0;
// Reserve pool_size_ buckets in the memory map.
memory_map_.reserve(pool_size_);
for (size_t i = 0; i < pool_size_; ++i) {
memory_map_.reserve(bounds_.pool_size_);
for (size_t i = 0; i < bounds_.pool_size_; ++i) {
memory_map_[memory_pool_[i]] = 0;
}
}
@ -52,30 +74,24 @@ public:
void ** borrow_handles(HandleType type, size_t number_of_handles)
{
switch (type) {
case HandleType::subscriber_handle:
if (number_of_handles > max_subscribers_) {
throw std::runtime_error("Requested size exceeded maximum subscribers.");
case HandleType::subscription_handle:
if (number_of_handles > bounds_.max_subscriptions_) {
throw std::runtime_error("Requested size exceeded maximum subscriptions.");
}
return subscriber_pool_;
return subscription_pool_;
case HandleType::service_handle:
if (number_of_handles > max_services_) {
if (number_of_handles > bounds_.max_services_) {
throw std::runtime_error("Requested size exceeded maximum services.");
}
return service_pool_;
case HandleType::client_handle:
if (number_of_handles > max_clients_) {
if (number_of_handles > bounds_.max_clients_) {
throw std::runtime_error("Requested size exceeded maximum clients.");
}
return client_pool_;
case HandleType::guard_condition_handle:
if (number_of_handles > max_guard_conditions_) {
throw std::runtime_error("Requested size exceeded maximum guard conditions.");
}
return guard_condition_pool_;
default:
break;
}
@ -86,17 +102,14 @@ public:
{
(void)handles;
switch (type) {
case HandleType::subscriber_handle:
memset(subscriber_pool_, 0, max_subscribers_);
case HandleType::subscription_handle:
memset(subscription_pool_, 0, bounds_.max_subscriptions_);
break;
case HandleType::service_handle:
memset(service_pool_, 0, max_services_);
memset(service_pool_, 0, bounds_.max_services_);
break;
case HandleType::client_handle:
memset(client_pool_, 0, max_clients_);
break;
case HandleType::guard_condition_handle:
memset(guard_condition_pool_, 0, max_guard_conditions_);
memset(client_pool_, 0, bounds_.max_clients_);
break;
default:
throw std::runtime_error("Unrecognized enum, could not return handle memory.");
@ -105,7 +118,7 @@ public:
executor::AnyExecutable::SharedPtr instantiate_next_executable()
{
if (exec_seq_ >= max_executables_) {
if (exec_seq_ >= bounds_.max_executables_) {
// wrap around
exec_seq_ = 0;
}
@ -119,7 +132,7 @@ public:
{
// Extremely naive static allocation strategy
// Keep track of block size at a given pointer
if (pool_seq_ + size > pool_size_) {
if (pool_seq_ + size > bounds_.pool_size_) {
// Start at 0
pool_seq_ = 0;
}
@ -145,19 +158,13 @@ public:
}
private:
static const size_t pool_size_ = 1024;
static const size_t max_subscribers_ = 10;
static const size_t max_services_ = 5;
static const size_t max_clients_ = 10;
static const size_t max_guard_conditions_ = 50;
static const size_t max_executables_ = 1;
ObjectPoolBounds bounds_;
void * memory_pool_[pool_size_];
void * subscriber_pool_[max_subscribers_];
void * service_pool_[max_services_];
void * client_pool_[max_clients_];
void * guard_condition_pool_[max_guard_conditions_];
executor::AnyExecutable executable_pool_[max_executables_];
void ** memory_pool_;
void ** subscription_pool_;
void ** service_pool_;
void ** client_pool_;
executor::AnyExecutable * executable_pool_;
size_t pool_seq_;
size_t exec_seq_;