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

code style only

Browse source
This commit is contained in:
Dirk Thomas 2015-04-07 10:57:33 -07:00
parent b4a2822749
commit f80b89a687
18 changed files with 393 additions and 493 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -28,18 +28,28 @@ namespace rclcpp
{
// Forward declarations for friend statement in class CallbackGroup
namespace node {class Node;}
namespace executor {class Executor;}
namespace node
{
class Node;
} // namespace node
namespace executor
{
class Executor;
} // namespace executor
namespace callback_group
{
enum class CallbackGroupType {MutuallyExclusive, Reentrant};
enum class CallbackGroupType {
MutuallyExclusive,
Reentrant
};
class CallbackGroup
{
friend class rclcpp::node::Node;
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup);

19
rclcpp/include/rclcpp/client.hpp
View file

@ -29,7 +29,10 @@ namespace rclcpp
{
// Forward declaration for friend statement
namespace executor {class Executor;}
namespace executor
{
class Executor;
} // namespace executor
namespace client
{
@ -37,18 +40,17 @@ namespace client
class ClientBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase);
ClientBase(rmw_client_t * client_handle,
const std::string &service_name)
ClientBase(rmw_client_t * client_handle, const std::string & service_name)
: client_handle_(client_handle), service_name_(service_name)
{}
~ClientBase()
{
if (client_handle_ != nullptr)
{
if (client_handle_ != nullptr) {
rmw_destroy_client(client_handle_);
client_handle_ = nullptr;
}
@ -66,8 +68,8 @@ public:
virtual std::shared_ptr<void> create_response() = 0;
virtual std::shared_ptr<void> create_request_header() = 0;
virtual void handle_response(std::shared_ptr<void> &request_header,
std::shared_ptr<void> &response) = 0;
virtual void handle_response(
std::shared_ptr<void> & request_header, std::shared_ptr<void> & response) = 0;
private:
RCLCPP_DISABLE_COPY(ClientBase);
@ -89,8 +91,7 @@ public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Client);
Client(rmw_client_t * client_handle,
const std::string& service_name)
Client(rmw_client_t * client_handle, const std::string & service_name)
: ClientBase(client_handle, service_name)
{}

6
rclcpp/include/rclcpp/contexts/default_context.hpp
View file

@ -33,8 +33,8 @@ public:
};
}
}
}
} // namespace default_context
} // namespace contexts
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */

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

@ -42,8 +42,7 @@ public:
virtual ~Executor()
{
if (interrupt_guard_condition_ != nullptr)
{
if (interrupt_guard_condition_ != nullptr) {
// TODO(wjwwood): Check ret code.
rmw_destroy_guard_condition(interrupt_guard_condition_);
}
@ -55,11 +54,9 @@ public:
add_node(rclcpp::node::Node::SharedPtr & node_ptr)
{
// Check to ensure node not already added
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (node == node_ptr)
{
if (node == node_ptr) {
// TODO: Use a different error here?
throw std::runtime_error(
"Cannot add node to executor, node already added.");
@ -75,7 +72,8 @@ public:
{
bool node_removed = false;
weak_nodes_.erase(
std::remove_if(weak_nodes_.begin(), weak_nodes_.end(),
std::remove_if(
weak_nodes_.begin(), weak_nodes_.end(),
[&](std::weak_ptr<rclcpp::node::Node> & i)
{
bool matched = (i.lock() == node_ptr);
@ -83,8 +81,7 @@ public:
return matched;
}));
// If the node was matched and removed, interrupt waiting
if (node_removed)
{
if (node_removed) {
rmw_trigger_guard_condition(interrupt_guard_condition_);
}
}
@ -94,8 +91,7 @@ public:
this->add_node(node);
// non-blocking = true
std::shared_ptr<AnyExecutable> any_exec;
while ((any_exec = get_next_executable(true)))
{
while ((any_exec = get_next_executable(true))) {
execute_any_executable(any_exec);
}
this->remove_node(node);
@ -118,24 +114,19 @@ protected:
void
execute_any_executable(std::shared_ptr<AnyExecutable> & any_exec)
{
if (!any_exec)
{
if (!any_exec) {
return;
}
if (any_exec->timer)
{
if (any_exec->timer) {
execute_timer(any_exec->timer);
}
if (any_exec->subscription)
{
if (any_exec->subscription) {
execute_subscription(any_exec->subscription);
}
if (any_exec->service)
{
if (any_exec->service) {
execute_service(any_exec->service);
}
if (any_exec->client)
{
if (any_exec->client) {
execute_client(any_exec->client);
}
// Reset the callback_group, regardless of type
@ -152,12 +143,9 @@ protected:
std::shared_ptr<void> message = subscription->create_message();
bool taken = false;
rmw_take(subscription->subscription_handle_, message.get(), &taken);
if (taken)
{
if (taken) {
subscription->handle_message(message);
}
else
{
} else {
std::cout << "[rclcpp::error] take failed for subscription on topic: "
<< subscription->get_topic_name()
<< std::endl;
@ -178,16 +166,14 @@ protected:
std::shared_ptr<void> request_header = service->create_request_header();
std::shared_ptr<void> request = service->create_request();
bool taken = false;
rmw_take_request(service->service_handle_,
rmw_take_request(
service->service_handle_,
request_header.get(),
request.get(),
&taken);
if (taken)
{
if (taken) {
service->handle_request(request_header, request);
}
else
{
} else {
std::cout << "[rclcpp::error] take failed for service on service: "
<< service->get_service_name()
<< std::endl;
@ -201,16 +187,14 @@ protected:
std::shared_ptr<void> request_header = client->create_request_header();
std::shared_ptr<void> response = client->create_response();
bool taken = false;
rmw_take_response(client->client_handle_,
rmw_take_response(
client->client_handle_,
request_header.get(),
response.get(),
&taken);
if (taken)
{
if (taken) {
client->handle_response(request_header, response);
}
else
{
} else {
std::cout << "[rclcpp::error] take failed for service on client" << std::endl;
}
}
@ -226,43 +210,35 @@ protected:
std::vector<rclcpp::timer::TimerBase::SharedPtr> timers;
std::vector<rclcpp::service::ServiceBase::SharedPtr> services;
std::vector<rclcpp::client::ClientBase::SharedPtr> clients;
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
has_invalid_weak_nodes = false;
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group || group->can_be_taken_from_.load() == false)
{
if (!group || !group->can_be_taken_from_.load()) {
continue;
}
for (auto &subscription : group->subscription_ptrs_)
{
for (auto & subscription : group->subscription_ptrs_) {
subs.push_back(subscription);
}
for (auto &timer : group->timer_ptrs_)
{
for (auto & timer : group->timer_ptrs_) {
timers.push_back(timer);
}
for (auto &service : group->service_ptrs_)
{
for (auto & service : group->service_ptrs_) {
services.push_back(service);
}
for (auto &client : group->client_ptrs_)
{
for (auto & client : group->client_ptrs_) {
clients.push_back(client);
}
}
}
// Clean up any invalid nodes, if they were detected
if (has_invalid_weak_nodes)
{
weak_nodes_.erase(remove_if(weak_nodes_.begin(), weak_nodes_.end(),
if (has_invalid_weak_nodes) {
weak_nodes_.erase(
remove_if(weak_nodes_.begin(), weak_nodes_.end(),
[](std::weak_ptr<rclcpp::node::Node> i)
{
return i.expired();
@ -275,15 +251,13 @@ protected:
// TODO(wjwwood): Avoid redundant malloc's
subscriber_handles.subscribers = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_subscriptions));
if (subscriber_handles.subscribers == NULL)
{
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.");
}
// Then fill the SubscriberHandles with ready subscriptions
size_t subscriber_handle_index = 0;
for (auto &subscription : subs)
{
for (auto & subscription : subs) {
subscriber_handles.subscribers[subscriber_handle_index] = \
subscription->subscription_handle_->data;
subscriber_handle_index += 1;
@ -296,15 +270,13 @@ protected:
// TODO(esteve): Avoid redundant malloc's
service_handles.services = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_services));
if (service_handles.services == NULL)
{
if (service_handles.services == NULL) {
// TODO(esteve): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for service pointers.");
}
// Then fill the ServiceHandles with ready services
size_t service_handle_index = 0;
for (auto &service : services)
{
for (auto & service : services) {
service_handles.services[service_handle_index] = \
service->service_handle_->data;
service_handle_index += 1;
@ -317,15 +289,13 @@ protected:
// TODO: Avoid redundant malloc's
client_handles.clients = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_clients));
if (client_handles.clients == NULL)
{
if (client_handles.clients == NULL) {
// TODO: Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for client pointers.");
}
// Then fill the ServiceHandles with ready clients
size_t client_handle_index = 0;
for (auto &client : clients)
{
for (auto & client : clients) {
client_handles.clients[client_handle_index] = \
client->client_handle_->data;
client_handle_index += 1;
@ -341,8 +311,7 @@ protected:
// TODO(wjwwood): Avoid redundant malloc's
guard_condition_handles.guard_conditions = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_guard_conds));
if (guard_condition_handles.guard_conditions == NULL)
{
if (guard_condition_handles.guard_conditions == NULL) {
// TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error(
"Could not malloc for guard condition pointers.");
@ -356,22 +325,21 @@ protected:
interrupt_guard_condition_->data;
// Then fill the SubscriberHandles with ready subscriptions
size_t guard_cond_handle_index = start_of_timer_guard_conds;
for (auto &timer : timers)
{
for (auto & timer : timers) {
guard_condition_handles.guard_conditions[guard_cond_handle_index] = \
timer->guard_condition_->data;
guard_cond_handle_index += 1;
}
// Now wait on the waitable subscriptions and timers
rmw_wait(&subscriber_handles,
rmw_wait(
&subscriber_handles,
&guard_condition_handles,
&service_handles,
&client_handles,
nonblocking);
// If ctrl-c guard condition, return directly
if (guard_condition_handles.guard_conditions[0] != 0)
{
if (guard_condition_handles.guard_conditions[0] != 0) {
// Make sure to free memory
// TODO(wjwwood): Remove theses when the "Avoid redundant malloc's"
// todo has been addressed.
@ -382,38 +350,30 @@ protected:
}
// Add the new work to the class's list of things waiting to be executed
// Starting with the subscribers
for (size_t i = 0; i < number_of_subscriptions; ++i)
{
for (size_t i = 0; i < number_of_subscriptions; ++i) {
void * handle = subscriber_handles.subscribers[i];
if (handle)
{
if (handle) {
subscriber_handles_.push_back(handle);
}
}
// Then the timers, start with start_of_timer_guard_conds
for (size_t i = start_of_timer_guard_conds; i < number_of_guard_conds; ++i)
{
for (size_t i = start_of_timer_guard_conds; i < number_of_guard_conds; ++i) {
void * handle = guard_condition_handles.guard_conditions[i];
if (handle)
{
if (handle) {
guard_condition_handles_.push_back(handle);
}
}
// Then the services
for (size_t i = 0; i < number_of_services; ++i)
{
for (size_t i = 0; i < number_of_services; ++i) {
void * handle = service_handles.services[i];
if (handle)
{
if (handle) {
service_handles_.push_back(handle);
}
}
// Then the clients
for (size_t i = 0; i < number_of_clients; ++i)
{
for (size_t i = 0; i < number_of_clients; ++i) {
void * handle = client_handles.clients[i];
if (handle)
{
if (handle) {
client_handles_.push_back(handle);
}
}
@ -431,24 +391,18 @@ 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)
{
if (!node) {
continue;
}
for (auto weak_group : node->callback_groups_)
{
for (auto weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group)
{
if (!group) {
continue;
}
for (auto subscription : group->subscription_ptrs_)
{
if (subscription->subscription_handle_->data == subscriber_handle)
{
for (auto subscription : group->subscription_ptrs_) {
if (subscription->subscription_handle_->data == subscriber_handle) {
return subscription;
}
}
@ -460,24 +414,18 @@ protected:
rclcpp::timer::TimerBase::SharedPtr
get_timer_by_handle(void * guard_condition_handle)
{
for (auto weak_node : weak_nodes_)
{
for (auto weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto weak_group : node->callback_groups_)
{
for (auto weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group)
{
if (!group) {
continue;
}
for (auto timer : group->timer_ptrs_)
{
if (timer->guard_condition_->data == guard_condition_handle)
{
for (auto timer : group->timer_ptrs_) {
if (timer->guard_condition_->data == guard_condition_handle) {
return timer;
}
}
@ -489,24 +437,18 @@ 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)
{
if (!node) {
continue;
}
for (auto weak_group : node->callback_groups_)
{
for (auto weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group)
{
if (!group) {
continue;
}
for (auto service : group->service_ptrs_)
{
if (service->service_handle_->data == service_handle)
{
for (auto service : group->service_ptrs_) {
if (service->service_handle_->data == service_handle) {
return service;
}
}
@ -518,24 +460,18 @@ 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)
{
if (!node) {
continue;
}
for (auto weak_group : node->callback_groups_)
{
for (auto weak_group : node->callback_groups_) {
auto group = weak_group.lock();
if (!group)
{
if (!group) {
continue;
}
for (auto client : group->client_ptrs_)
{
if (client->client_handle_->data == client_handle)
{
for (auto client : group->client_ptrs_) {
if (client->client_handle_->data == client_handle) {
return client;
}
}
@ -572,26 +508,20 @@ protected:
rclcpp::node::Node::SharedPtr
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr & group)
{
if (!group)
{
if (!group) {
return rclcpp::node::Node::SharedPtr();
}
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto callback_group = weak_group.lock();
if (!callback_group)
{
if (!callback_group) {
continue;
}
if (callback_group == group)
{
if (callback_group == group) {
return node;
}
}
@ -603,20 +533,15 @@ protected:
get_group_by_timer(
rclcpp::timer::TimerBase::SharedPtr & timer)
{
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
for (auto &t : group->timer_ptrs_)
{
if (t == timer)
{
for (auto & t : group->timer_ptrs_) {
if (t == timer) {
return group;
}
}
@ -628,22 +553,18 @@ protected:
void
get_next_timer(std::shared_ptr<AnyExecutable> & any_exec)
{
for (auto handle : guard_condition_handles_)
{
for (auto handle : guard_condition_handles_) {
auto timer = get_timer_by_handle(handle);
if (timer)
{
if (timer) {
// Find the group for this handle and see if it can be serviced
auto group = get_group_by_timer(timer);
if (!group)
{
if (!group) {
// Group was not found, meaning the timer is not valid...
// Remove it from the ready list and continue looking
remove_guard_condition_handle_from_guard_condition_handles(handle);
continue;
}
if (!group->can_be_taken_from_.load())
{
if (!group->can_be_taken_from_.load()) {
// Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching
continue;
@ -664,20 +585,15 @@ protected:
get_group_by_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr & subscription)
{
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
for (auto &sub : group->subscription_ptrs_)
{
if (sub == subscription)
{
for (auto & sub : group->subscription_ptrs_) {
if (sub == subscription) {
return group;
}
}
@ -689,22 +605,18 @@ protected:
void
get_next_subscription(std::shared_ptr<AnyExecutable> & any_exec)
{
for (auto handle : subscriber_handles_)
{
for (auto handle : subscriber_handles_) {
auto subscription = get_subscription_by_handle(handle);
if (subscription)
{
if (subscription) {
// Find the group for this handle and see if it can be serviced
auto group = get_group_by_subscription(subscription);
if (!group)
{
if (!group) {
// Group was not found, meaning the subscription is not valid...
// Remove it from the ready list and continue looking
remove_subscriber_handle_from_subscriber_handles(handle);
continue;
}
if (!group->can_be_taken_from_.load())
{
if (!group->can_be_taken_from_.load()) {
// Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching
continue;
@ -725,20 +637,15 @@ protected:
get_group_by_service(
rclcpp::service::ServiceBase::SharedPtr & service)
{
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
for (auto &serv : group->service_ptrs_)
{
if (serv == service)
{
for (auto & serv : group->service_ptrs_) {
if (serv == service) {
return group;
}
}
@ -750,22 +657,18 @@ protected:
void
get_next_service(std::shared_ptr<AnyExecutable> & any_exec)
{
for (auto handle : service_handles_)
{
for (auto handle : service_handles_) {
auto service = get_service_by_handle(handle);
if (service)
{
if (service) {
// Find the group for this handle and see if it can be serviced
auto group = get_group_by_service(service);
if (!group)
{
if (!group) {
// Group was not found, meaning the service is not valid...
// Remove it from the ready list and continue looking
remove_service_handle_from_service_handles(handle);
continue;
}
if (!group->can_be_taken_from_.load())
{
if (!group->can_be_taken_from_.load()) {
// Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching
continue;
@ -786,20 +689,15 @@ protected:
get_group_by_client(
rclcpp::client::ClientBase::SharedPtr & client)
{
for (auto &weak_node : weak_nodes_)
{
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node)
{
if (!node) {
continue;
}
for (auto &weak_group : node->callback_groups_)
{
for (auto & weak_group : node->callback_groups_) {
auto group = weak_group.lock();
for (auto &cli : group->client_ptrs_)
{
if (cli == client)
{
for (auto & cli : group->client_ptrs_) {
if (cli == client) {
return group;
}
}
@ -811,22 +709,18 @@ protected:
void
get_next_client(std::shared_ptr<AnyExecutable> & any_exec)
{
for (auto handle : client_handles_)
{
for (auto handle : client_handles_) {
auto client = get_client_by_handle(handle);
if (client)
{
if (client) {
// Find the group for this handle and see if it can be serviced
auto group = get_group_by_client(client);
if (!group)
{
if (!group) {
// Group was not found, meaning the service is not valid...
// Remove it from the ready list and continue looking
remove_client_handle_from_client_handles(handle);
continue;
}
if (!group->can_be_taken_from_.load())
{
if (!group->can_be_taken_from_.load()) {
// Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching
continue;
@ -849,26 +743,22 @@ protected:
std::shared_ptr<AnyExecutable> any_exec(new AnyExecutable());
// Check the timers to see if there are any that are ready, if so return
get_next_timer(any_exec);
if (any_exec->timer)
{
if (any_exec->timer) {
return any_exec;
}
// Check the subscriptions to see if there are any that are ready
get_next_subscription(any_exec);
if (any_exec->subscription)
{
if (any_exec->subscription) {
return any_exec;
}
// Check the services to see if there are any that are ready
get_next_service(any_exec);
if (any_exec->service)
{
if (any_exec->service) {
return any_exec;
}
// Check the clients to see if there are any that are ready
get_next_client(any_exec);
if (any_exec->client)
{
if (any_exec->client) {
return any_exec;
}
// If there is neither a ready timer nor subscription, return a null ptr
@ -883,8 +773,7 @@ protected:
// TODO(wjwwood): improve run to run efficiency of this function
auto any_exec = get_next_ready_executable();
// If there are none
if (!any_exec)
{
if (!any_exec) {
// Wait for subscriptions or timers to work on
wait_for_work(nonblocking);
// Try again
@ -892,15 +781,13 @@ protected:
}
// 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
if (any_exec)
{
if (any_exec) {
// If it is valid, check to see if the group is mutually exclusive or
// not, then mark it accordingly
if (any_exec->callback_group->type_ == \
callback_group::CallbackGroupType::MutuallyExclusive)
{
callback_group::CallbackGroupType::MutuallyExclusive) {
// It should not have been taken otherwise
assert(any_exec->callback_group->can_be_taken_from_.load() == true);
assert(any_exec->callback_group->can_be_taken_from_.load());
// Set to false to indicate something is being run from this group
any_exec->callback_group->can_be_taken_from_.store(false);
}

4
rclcpp/include/rclcpp/executors.hpp
View file

@ -26,7 +26,7 @@ namespace executors
using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
}
}
} // namespace executors
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */

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

@ -43,8 +43,7 @@ public:
MultiThreadedExecutor()
{
number_of_threads_ = std::thread::hardware_concurrency();
if (number_of_threads_ == 0)
{
if (number_of_threads_ == 0) {
number_of_threads_ = 1;
}
}
@ -58,15 +57,13 @@ public:
{
std::lock_guard<std::mutex> wait_lock(wait_mutex_);
size_t thread_id_ = 1; // Use a _ suffix to avoid shadowing `rclcpp::thread_id`
for (size_t i = number_of_threads_; i > 0; --i)
{
for (size_t i = number_of_threads_; i > 0; --i) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id_++);
threads.emplace_back(func);
}
}
for (auto &thread : threads)
{
for (auto & thread : threads) {
thread.join();
}
}
@ -81,13 +78,11 @@ private:
void run(size_t this_thread_id)
{
rclcpp::thread_id = this_thread_id;
while (rclcpp::utilities::ok())
{
while (rclcpp::utilities::ok()) {
std::shared_ptr<AnyExecutable> any_exec;
{
std::lock_guard<std::mutex> wait_lock(wait_mutex_);
if (!rclcpp::utilities::ok())
{
if (!rclcpp::utilities::ok()) {
return;
}
any_exec = get_next_executable();

3
rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
View file

@ -46,8 +46,7 @@ public:
void spin()
{
while (rclcpp::utilities::ok())
{
while (rclcpp::utilities::ok()) {
auto any_exec = get_next_executable();
execute_any_executable(any_exec);
}

18
rclcpp/include/rclcpp/node.hpp
View file

@ -29,13 +29,19 @@
#include <rclcpp/timer.hpp>
// Forward declaration of ROS middleware class
namespace rmw {struct rmw_node_t;}
namespace rmw
{
struct rmw_node_t;
} // namespace rmw
namespace rclcpp
{
// Forward declaration for friend statement
namespace executor {class Executor;}
namespace executor
{
class Executor;
} // namespace executor
namespace node
{
@ -47,6 +53,7 @@ namespace node
class Node
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Node);
@ -64,7 +71,8 @@ public:
create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
/* Create and return a Publisher. */
template<typename MessageT> rclcpp::publisher::Publisher::SharedPtr
template<typename MessageT>
rclcpp::publisher::Publisher::SharedPtr
create_publisher(std::string topic_name, size_t queue_size);
/* Create and return a Subscription. */
@ -138,7 +146,9 @@ private:
rclcpp::node::Node::SharedPtr \
create_node() \
{ \
return rclcpp::node::Node::SharedPtr(new Class(rclcpp::contexts::default_context::DefaultContext::make_shared())); \
return rclcpp::node::Node::SharedPtr(new Class( \
rclcpp::contexts::default_context::DefaultContext:: \
make_shared())); \
}
#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_

58
rclcpp/include/rclcpp/node_impl.hpp
View file

@ -74,11 +74,9 @@ bool
Node::group_in_node(callback_group::CallbackGroup::SharedPtr & group)
{
bool group_belongs_to_this_node = false;
for (auto &weak_group : this->callback_groups_)
{
for (auto & weak_group : this->callback_groups_) {
auto cur_group = weak_group.lock();
if (cur_group && cur_group == group)
{
if (cur_group && (cur_group == group)) {
group_belongs_to_this_node = true;
}
}
@ -100,21 +98,18 @@ Node::create_subscription(
using namespace rclcpp::subscription;
auto sub = Subscription<MessageT>::make_shared(subscriber_handle,
auto sub = Subscription<MessageT>::make_shared(
subscriber_handle,
topic_name,
callback);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
if (group)
{
if (!group_in_node(group))
{
if (group) {
if (!group_in_node(group)) {
// TODO: use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
group->add_subscription(sub_base_ptr);
}
else
{
} else {
default_callback_group_->add_subscription(sub_base_ptr);
}
number_of_subscriptions_++;
@ -122,22 +117,19 @@ Node::create_subscription(
}
rclcpp::timer::WallTimer::SharedPtr
Node::create_wall_timer(std::chrono::nanoseconds period,
Node::create_wall_timer(
std::chrono::nanoseconds period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
auto timer = rclcpp::timer::WallTimer::make_shared(period, callback);
if (group)
{
if (!group_in_node(group))
{
if (group) {
if (!group_in_node(group)) {
// TODO: use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
group->add_timer(timer);
}
else
{
} else {
default_callback_group_->add_timer(timer);
}
number_of_timers_++;
@ -171,21 +163,18 @@ Node::create_client(
using namespace rclcpp::client;
auto cli = Client<ServiceT>::make_shared(client_handle,
auto cli = Client<ServiceT>::make_shared(
client_handle,
service_name);
auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
if (group)
{
if (!group_in_node(group))
{
if (group) {
if (!group_in_node(group)) {
// TODO(esteve): use custom exception
throw std::runtime_error("Cannot create client, group not in node.");
}
group->add_client(cli_base_ptr);
}
else
{
} else {
default_callback_group_->add_client(cli_base_ptr);
}
number_of_clients_++;
@ -210,22 +199,19 @@ Node::create_service(
using namespace rclcpp::service;
auto serv = Service<ServiceT>::make_shared(service_handle,
auto serv = Service<ServiceT>::make_shared(
service_handle,
service_name,
callback);
auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
if (group)
{
if (!group_in_node(group))
{
if (group) {
if (!group_in_node(group)) {
// TODO: use custom exception
throw std::runtime_error("Cannot create service, group not in node.");
}
group->add_service(serv_base_ptr);
}
else
{
} else {
default_callback_group_->add_service(serv_base_ptr);
}
number_of_services_++;

8
rclcpp/include/rclcpp/publisher.hpp
View file

@ -25,7 +25,10 @@ namespace rclcpp
{
// Forward declaration for friend statement
namespace node {class Node;}
namespace node
{
class Node;
} // namespace node
namespace publisher
{
@ -39,7 +42,8 @@ public:
: publisher_handle_(publisher_handle)
{}
template<typename MessageT> void
template<typename MessageT>
void
publish(std::shared_ptr<MessageT> & msg)
{
rmw_publish(publisher_handle_, msg.get());

13
rclcpp/include/rclcpp/rate.hpp
View file

@ -63,8 +63,7 @@ public:
// Time of next interval
auto next_interval = last_interval_ + period_;
// Detect backwards time flow
if (now < last_interval_)
{
if (now < last_interval_) {
// Best thing to do is to set the next_interval to now + period
next_interval = now + period_;
}
@ -73,13 +72,11 @@ public:
// Update the interval
last_interval_ += period_;
// If the time_to_sleep is negative or zero, don't sleep
if (time_to_sleep <= std::chrono::seconds(0))
{
if (time_to_sleep <= std::chrono::seconds(0)) {
// If an entire cycle was missed then reset next interval.
// This might happen if the loop took more than a cycle.
// Or if time jumps forward.
if (now > next_interval + period_)
{
if (now > next_interval + period_) {
last_interval_ = now + period_;
}
// Either way do not sleep and return false
@ -113,7 +110,7 @@ private:
typedef GenericRate<std::chrono::system_clock> Rate;
typedef GenericRate<std::chrono::steady_clock> WallRate;
}
}
} // namespace rate
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_RATE_HPP_ */

21
rclcpp/include/rclcpp/service.hpp
View file

@ -28,7 +28,10 @@ namespace rclcpp
{
// Forward declaration for friend statement
namespace executor {class Executor;}
namespace executor
{
class Executor;
} // namespace executor
namespace service
{
@ -36,18 +39,19 @@ namespace service
class ServiceBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase);
ServiceBase(rmw_service_t * service_handle,
ServiceBase(
rmw_service_t * service_handle,
const std::string service_name)
: service_handle_(service_handle), service_name_(service_name)
{}
~ServiceBase()
{
if (service_handle_ != nullptr)
{
if (service_handle_ != nullptr) {
rmw_destroy_service(service_handle_);
service_handle_ = nullptr;
}
@ -65,7 +69,8 @@ public:
virtual std::shared_ptr<void> create_request() = 0;
virtual std::shared_ptr<void> create_request_header() = 0;
virtual void handle_request(std::shared_ptr<void> &request_header,
virtual void handle_request(
std::shared_ptr<void> & request_header,
std::shared_ptr<void> & request) = 0;
private:
@ -85,7 +90,8 @@ public:
std::shared_ptr<typename ServiceT::Response> &)> CallbackType;
RCLCPP_MAKE_SHARED_DEFINITIONS(Service);
Service(rmw_service_t * service_handle,
Service(
rmw_service_t * service_handle,
const std::string & service_name,
CallbackType callback)
: ServiceBase(service_handle, service_name), callback_(callback)
@ -112,7 +118,8 @@ public:
send_response(typed_request_header, response);
}
void send_response(std::shared_ptr<rmw_request_id_t> &req_id,
void send_response(
std::shared_ptr<rmw_request_id_t> & req_id,
std::shared_ptr<typename ServiceT::Response> & response)
{
rmw_send_response(get_service_handle(), req_id.get(), response.get());

9
rclcpp/include/rclcpp/subscription.hpp
View file

@ -27,7 +27,10 @@ namespace rclcpp
{
// Forward declaration is for friend statement in SubscriptionBase
namespace executor {class Executor;}
namespace executor
{
class Executor;
} // namespace executor
namespace subscription
{
@ -35,6 +38,7 @@ namespace subscription
class SubscriptionBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase);
@ -67,7 +71,8 @@ public:
typedef std::function<void (const std::shared_ptr<MessageT> &)> CallbackType;
RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription);
Subscription(rmw_subscription_t * subscription_handle,
Subscription(
rmw_subscription_t * subscription_handle,
std::string & topic_name,
CallbackType callback)
: SubscriptionBase(subscription_handle, topic_name), callback_(callback)

13
rclcpp/include/rclcpp/timer.hpp
View file

@ -30,7 +30,10 @@ namespace rclcpp
{
// Forward declaration is for friend statement in GenericTimer
namespace executor {class Executor;}
namespace executor
{
class Executor;
} // namespace executor
namespace timer
{
@ -40,6 +43,7 @@ typedef std::function<void()> CallbackType;
class TimerBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(TimerBase);
@ -72,6 +76,7 @@ template<class Clock = std::chrono::high_resolution_clock>
class GenericTimer : public TimerBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(GenericTimer);
@ -89,11 +94,9 @@ public:
void
run()
{
while (rclcpp::utilities::ok() && !this->canceled_)
{
while (rclcpp::utilities::ok() && !this->canceled_) {
loop_rate_.sleep();
if (!rclcpp::utilities::ok())
{
if (!rclcpp::utilities::ok()) {
return;
}
rmw_trigger_guard_condition(guard_condition_);

22
rclcpp/include/rclcpp/utilities.hpp
View file

@ -58,25 +58,21 @@ namespace
// TODO(wjwwood): remove
std::cout << "signal_handler(" << signal_value << ")" << std::endl;
#ifdef HAS_SIGACTION
if (old_action.sa_flags & SA_SIGINFO)
{
if (old_action.sa_sigaction != NULL)
{
if (old_action.sa_flags & SA_SIGINFO) {
if (old_action.sa_sigaction != NULL) {
old_action.sa_sigaction(signal_value, siginfo, context);
}
}
else
{
if (old_action.sa_handler != NULL && // Is set
} else {
if (
old_action.sa_handler != NULL && // Is set
old_action.sa_handler != SIG_DFL && // Is not default
old_action.sa_handler != SIG_IGN) // Is not ignored
{
old_action.sa_handler != SIG_IGN // Is not ignored
) {
old_action.sa_handler(signal_value);
}
}
#else
if (old_signal_handler)
{
if (old_signal_handler) {
old_signal_handler(signal_value);
}
#endif
@ -84,7 +80,7 @@ namespace
rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
g_interrupt_condition_variable.notify_all();
}
}
} // namespace
namespace rclcpp
{