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

Merge pull request #18 from ros2/code_style_uncrustify

Browse source 
code style only
This commit is contained in:
Dirk Thomas 2015-04-07 17:05:05 -07:00
commit 0c0bb8a186
18 changed files with 412 additions and 505 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 // Forward declarations for friend statement in class CallbackGroup
namespace node {class Node;} namespace node
namespace executor {class Executor;} {
class Node;
} // namespace node
namespace executor
{
class Executor;
} // namespace executor
namespace callback_group namespace callback_group
{ {
enum class CallbackGroupType {MutuallyExclusive, Reentrant}; enum class CallbackGroupType {
MutuallyExclusive,
Reentrant
};
class CallbackGroup class CallbackGroup
{ {
friend class rclcpp::node::Node; friend class rclcpp::node::Node;
friend class rclcpp::executor::Executor; friend class rclcpp::executor::Executor;
public: public:
RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup); RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup);

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

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

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

@ -38,12 +38,13 @@ class Executor
public: public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Executor); RCLCPP_MAKE_SHARED_DEFINITIONS(Executor);
Executor() : interrupt_guard_condition_(rmw_create_guard_condition()) {} Executor()
: interrupt_guard_condition_(rmw_create_guard_condition())
{}
virtual ~Executor() virtual ~Executor()
{ {
if (interrupt_guard_condition_ != nullptr) if (interrupt_guard_condition_ != nullptr) {
{
// TODO(wjwwood): Check ret code. // TODO(wjwwood): Check ret code.
rmw_destroy_guard_condition(interrupt_guard_condition_); rmw_destroy_guard_condition(interrupt_guard_condition_);
} }
@ -55,14 +56,11 @@ public:
add_node(rclcpp::node::Node::SharedPtr & node_ptr) add_node(rclcpp::node::Node::SharedPtr & node_ptr)
{ {
// Check to ensure node not already added // Check to ensure node not already added
for (auto &weak_node : weak_nodes_) for (auto & weak_node : weak_nodes_) {
{
auto node = weak_node.lock(); auto node = weak_node.lock();
if (node == node_ptr) if (node == node_ptr) {
{
// TODO: Use a different error here? // TODO: Use a different error here?
throw std::runtime_error( throw std::runtime_error("Cannot add node to executor, node already added.");
"Cannot add node to executor, node already added.");
} }
} }
weak_nodes_.push_back(node_ptr); weak_nodes_.push_back(node_ptr);
@ -75,7 +73,8 @@ public:
{ {
bool node_removed = false; bool node_removed = false;
weak_nodes_.erase( 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) [&](std::weak_ptr<rclcpp::node::Node> & i)
{ {
bool matched = (i.lock() == node_ptr); bool matched = (i.lock() == node_ptr);
@ -83,8 +82,7 @@ public:
return matched; return matched;
})); }));
// If the node was matched and removed, interrupt waiting // If the node was matched and removed, interrupt waiting
if (node_removed) if (node_removed) {
{
rmw_trigger_guard_condition(interrupt_guard_condition_); rmw_trigger_guard_condition(interrupt_guard_condition_);
} }
} }
@ -94,8 +92,7 @@ public:
this->add_node(node); this->add_node(node);
// non-blocking = true // non-blocking = true
std::shared_ptr<AnyExecutable> any_exec; 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); execute_any_executable(any_exec);
} }
this->remove_node(node); this->remove_node(node);
@ -104,7 +101,9 @@ public:
protected: protected:
struct AnyExecutable struct AnyExecutable
{ {
AnyExecutable() : subscription(0), timer(0), callback_group(0), node(0) {} AnyExecutable()
: subscription(0), timer(0), callback_group(0), node(0)
{}
// Either the subscription or the timer will be set, but not both // Either the subscription or the timer will be set, but not both
rclcpp::subscription::SubscriptionBase::SharedPtr subscription; rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
rclcpp::timer::TimerBase::SharedPtr timer; rclcpp::timer::TimerBase::SharedPtr timer;
@ -118,24 +117,19 @@ protected:
void void
execute_any_executable(std::shared_ptr<AnyExecutable> & any_exec) execute_any_executable(std::shared_ptr<AnyExecutable> & any_exec)
{ {
if (!any_exec) if (!any_exec) {
{
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->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
@ -152,12 +146,9 @@ protected:
std::shared_ptr<void> message = subscription->create_message(); std::shared_ptr<void> message = subscription->create_message();
bool taken = false; bool taken = false;
rmw_take(subscription->subscription_handle_, message.get(), &taken); rmw_take(subscription->subscription_handle_, message.get(), &taken);
if (taken) if (taken) {
{
subscription->handle_message(message); subscription->handle_message(message);
} } else {
else
{
std::cout << "[rclcpp::error] take failed for subscription on topic: " std::cout << "[rclcpp::error] take failed for subscription on topic: "
<< subscription->get_topic_name() << subscription->get_topic_name()
<< std::endl; << std::endl;
@ -178,16 +169,14 @@ protected:
std::shared_ptr<void> request_header = service->create_request_header(); std::shared_ptr<void> request_header = service->create_request_header();
std::shared_ptr<void> request = service->create_request(); std::shared_ptr<void> request = service->create_request();
bool taken = false; bool taken = false;
rmw_take_request(service->service_handle_, rmw_take_request(
service->service_handle_,
request_header.get(), request_header.get(),
request.get(), request.get(),
&taken); &taken);
if (taken) if (taken) {
{
service->handle_request(request_header, request); service->handle_request(request_header, request);
} } else {
else
{
std::cout << "[rclcpp::error] take failed for service on service: " std::cout << "[rclcpp::error] take failed for service on service: "
<< service->get_service_name() << service->get_service_name()
<< std::endl; << std::endl;
@ -201,16 +190,14 @@ protected:
std::shared_ptr<void> request_header = client->create_request_header(); std::shared_ptr<void> request_header = client->create_request_header();
std::shared_ptr<void> response = client->create_response(); std::shared_ptr<void> response = client->create_response();
bool taken = false; bool taken = false;
rmw_take_response(client->client_handle_, rmw_take_response(
client->client_handle_,
request_header.get(), request_header.get(),
response.get(), response.get(),
&taken); &taken);
if (taken) if (taken) {
{
client->handle_response(request_header, response); client->handle_response(request_header, response);
} } else {
else
{
std::cout << "[rclcpp::error] take failed for service on client" << std::endl; std::cout << "[rclcpp::error] take failed for service on client" << std::endl;
} }
} }
@ -226,43 +213,35 @@ protected:
std::vector<rclcpp::timer::TimerBase::SharedPtr> timers; std::vector<rclcpp::timer::TimerBase::SharedPtr> timers;
std::vector<rclcpp::service::ServiceBase::SharedPtr> services; std::vector<rclcpp::service::ServiceBase::SharedPtr> services;
std::vector<rclcpp::client::ClientBase::SharedPtr> clients; std::vector<rclcpp::client::ClientBase::SharedPtr> clients;
for (auto &weak_node : weak_nodes_) for (auto & weak_node : weak_nodes_) {
{
auto node = weak_node.lock(); auto node = weak_node.lock();
if (!node) if (!node) {
{
has_invalid_weak_nodes = false; has_invalid_weak_nodes = false;
continue; continue;
} }
for (auto &weak_group : node->callback_groups_) for (auto & weak_group : node->callback_groups_) {
{
auto group = weak_group.lock(); auto group = weak_group.lock();
if (!group || group->can_be_taken_from_.load() == false) if (!group || !group->can_be_taken_from_.load()) {
{
continue; continue;
} }
for (auto &subscription : group->subscription_ptrs_) for (auto & subscription : group->subscription_ptrs_) {
{
subs.push_back(subscription); subs.push_back(subscription);
} }
for (auto &timer : group->timer_ptrs_) for (auto & timer : group->timer_ptrs_) {
{
timers.push_back(timer); timers.push_back(timer);
} }
for (auto &service : group->service_ptrs_) for (auto & service : group->service_ptrs_) {
{
services.push_back(service); services.push_back(service);
} }
for (auto &client : group->client_ptrs_) for (auto & client : group->client_ptrs_) {
{
clients.push_back(client); clients.push_back(client);
} }
} }
} }
// Clean up any invalid nodes, if they were detected // Clean up any invalid nodes, if they were detected
if (has_invalid_weak_nodes) if (has_invalid_weak_nodes) {
{ weak_nodes_.erase(
weak_nodes_.erase(remove_if(weak_nodes_.begin(), weak_nodes_.end(), remove_if(weak_nodes_.begin(), weak_nodes_.end(),
[](std::weak_ptr<rclcpp::node::Node> i) [](std::weak_ptr<rclcpp::node::Node> i)
{ {
return i.expired(); return i.expired();
@ -275,15 +254,13 @@ protected:
// TODO(wjwwood): Avoid redundant malloc's // TODO(wjwwood): Avoid redundant malloc's
subscriber_handles.subscribers = static_cast<void **>( subscriber_handles.subscribers = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_subscriptions)); 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? // TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for subscriber pointers."); throw std::runtime_error("Could not malloc for subscriber pointers.");
} }
// Then fill the SubscriberHandles with ready subscriptions // Then fill the SubscriberHandles with ready subscriptions
size_t subscriber_handle_index = 0; size_t subscriber_handle_index = 0;
for (auto &subscription : subs) for (auto & subscription : subs) {
{
subscriber_handles.subscribers[subscriber_handle_index] = \ subscriber_handles.subscribers[subscriber_handle_index] = \
subscription->subscription_handle_->data; subscription->subscription_handle_->data;
subscriber_handle_index += 1; subscriber_handle_index += 1;
@ -296,15 +273,13 @@ protected:
// TODO(esteve): Avoid redundant malloc's // TODO(esteve): Avoid redundant malloc's
service_handles.services = static_cast<void **>( service_handles.services = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_services)); 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? // TODO(esteve): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for service pointers."); throw std::runtime_error("Could not malloc for service pointers.");
} }
// Then fill the ServiceHandles with ready services // Then fill the ServiceHandles with ready services
size_t service_handle_index = 0; size_t service_handle_index = 0;
for (auto &service : services) for (auto & service : services) {
{
service_handles.services[service_handle_index] = \ service_handles.services[service_handle_index] = \
service->service_handle_->data; service->service_handle_->data;
service_handle_index += 1; service_handle_index += 1;
@ -317,15 +292,13 @@ protected:
// TODO: Avoid redundant malloc's // TODO: Avoid redundant malloc's
client_handles.clients = static_cast<void **>( client_handles.clients = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_clients)); 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? // TODO: Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for client pointers."); throw std::runtime_error("Could not malloc for client pointers.");
} }
// Then fill the ServiceHandles with ready clients // Then fill the ServiceHandles with ready clients
size_t client_handle_index = 0; size_t client_handle_index = 0;
for (auto &client : clients) for (auto & client : clients) {
{
client_handles.clients[client_handle_index] = \ client_handles.clients[client_handle_index] = \
client->client_handle_->data; client->client_handle_->data;
client_handle_index += 1; client_handle_index += 1;
@ -341,11 +314,9 @@ protected:
// TODO(wjwwood): Avoid redundant malloc's // TODO(wjwwood): Avoid redundant malloc's
guard_condition_handles.guard_conditions = static_cast<void **>( guard_condition_handles.guard_conditions = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_guard_conds)); 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? // TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error( throw std::runtime_error("Could not malloc for guard condition pointers.");
"Could not malloc for guard condition pointers.");
} }
// Put the global ctrl-c guard condition in // Put the global ctrl-c guard condition in
assert(guard_condition_handles.guard_condition_count > 1); assert(guard_condition_handles.guard_condition_count > 1);
@ -356,22 +327,21 @@ protected:
interrupt_guard_condition_->data; interrupt_guard_condition_->data;
// Then fill the SubscriberHandles with ready subscriptions // Then fill the SubscriberHandles with ready subscriptions
size_t guard_cond_handle_index = start_of_timer_guard_conds; 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] = \ guard_condition_handles.guard_conditions[guard_cond_handle_index] = \
timer->guard_condition_->data; timer->guard_condition_->data;
guard_cond_handle_index += 1; guard_cond_handle_index += 1;
} }
// Now wait on the waitable subscriptions and timers // Now wait on the waitable subscriptions and timers
rmw_wait(&subscriber_handles, rmw_wait(
&subscriber_handles,
&guard_condition_handles, &guard_condition_handles,
&service_handles, &service_handles,
&client_handles, &client_handles,
nonblocking); nonblocking);
// If ctrl-c guard condition, return directly // 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 // Make sure to free memory
// TODO(wjwwood): Remove theses when the "Avoid redundant malloc's" // TODO(wjwwood): Remove theses when the "Avoid redundant malloc's"
// todo has been addressed. // todo has been addressed.
@ -382,38 +352,30 @@ protected:
} }
// Add the new work to the class's list of things waiting to be executed // Add the new work to the class's list of things waiting to be executed
// Starting with the subscribers // 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]; void * handle = subscriber_handles.subscribers[i];
if (handle) if (handle) {
{
subscriber_handles_.push_back(handle); subscriber_handles_.push_back(handle);
} }
} }
// Then the timers, start with start_of_timer_guard_conds // 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]; void * handle = guard_condition_handles.guard_conditions[i];
if (handle) if (handle) {
{
guard_condition_handles_.push_back(handle); guard_condition_handles_.push_back(handle);
} }
} }
// Then the services // 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]; void * handle = service_handles.services[i];
if (handle) if (handle) {
{
service_handles_.push_back(handle); service_handles_.push_back(handle);
} }
} }
// Then the clients // 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]; void * handle = client_handles.clients[i];
if (handle) if (handle) {
{
client_handles_.push_back(handle); client_handles_.push_back(handle);
} }
} }
@ -431,24 +393,18 @@ protected:
rclcpp::subscription::SubscriptionBase::SharedPtr rclcpp::subscription::SubscriptionBase::SharedPtr
get_subscription_by_handle(void * subscriber_handle) get_subscription_by_handle(void * subscriber_handle)
{ {
for (auto weak_node : weak_nodes_) for (auto weak_node : weak_nodes_) {
{
auto node = weak_node.lock(); auto node = weak_node.lock();
if (!node) if (!node) {
{
continue; continue;
} }
for (auto weak_group : node->callback_groups_) for (auto weak_group : node->callback_groups_) {
{
auto group = weak_group.lock(); auto group = weak_group.lock();
if (!group) if (!group) {
{
continue; continue;
} }
for (auto subscription : group->subscription_ptrs_) for (auto subscription : group->subscription_ptrs_) {
{ if (subscription->subscription_handle_->data == subscriber_handle) {
if (subscription->subscription_handle_->data == subscriber_handle)
{
return subscription; return subscription;
} }
} }
@ -460,24 +416,18 @@ protected:
rclcpp::timer::TimerBase::SharedPtr rclcpp::timer::TimerBase::SharedPtr
get_timer_by_handle(void * guard_condition_handle) 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(); auto node = weak_node.lock();
if (!node) if (!node) {
{
continue; continue;
} }
for (auto weak_group : node->callback_groups_) for (auto weak_group : node->callback_groups_) {
{
auto group = weak_group.lock(); auto group = weak_group.lock();
if (!group) if (!group) {
{
continue; continue;
} }
for (auto timer : group->timer_ptrs_) for (auto timer : group->timer_ptrs_) {
{ if (timer->guard_condition_->data == guard_condition_handle) {
if (timer->guard_condition_->data == guard_condition_handle)
{
return timer; return timer;
} }
} }
@ -489,24 +439,18 @@ protected:
rclcpp::service::ServiceBase::SharedPtr rclcpp::service::ServiceBase::SharedPtr
get_service_by_handle(void * service_handle) get_service_by_handle(void * service_handle)
{ {
for (auto weak_node : weak_nodes_) for (auto weak_node : weak_nodes_) {
{
auto node = weak_node.lock(); auto node = weak_node.lock();
if (!node) if (!node) {
{
continue; continue;
} }
for (auto weak_group : node->callback_groups_) for (auto weak_group : node->callback_groups_) {
{
auto group = weak_group.lock(); auto group = weak_group.lock();
if (!group) if (!group) {
{
continue; continue;
} }
for (auto service : group->service_ptrs_) for (auto service : group->service_ptrs_) {
{ if (service->service_handle_->data == service_handle) {
if (service->service_handle_->data == service_handle)
{
return service; return service;
} }
} }
@ -518,24 +462,18 @@ protected:
rclcpp::client::ClientBase::SharedPtr rclcpp::client::ClientBase::SharedPtr
get_client_by_handle(void * client_handle) get_client_by_handle(void * client_handle)
{ {
for (auto weak_node : weak_nodes_) for (auto weak_node : weak_nodes_) {
{
auto node = weak_node.lock(); auto node = weak_node.lock();
if (!node) if (!node) {
{
continue; continue;
} }
for (auto weak_group : node->callback_groups_) for (auto weak_group : node->callback_groups_) {
{
auto group = weak_group.lock(); auto group = weak_group.lock();
if (!group) if (!group) {
{
continue; continue;
} }
for (auto client : group->client_ptrs_) for (auto client : group->client_ptrs_) {
{ if (client->client_handle_->data == client_handle) {
if (client->client_handle_->data == client_handle)
{
return client; return client;
} }
} }
@ -572,26 +510,20 @@ protected:
rclcpp::node::Node::SharedPtr rclcpp::node::Node::SharedPtr
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr & group) get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr & group)
{ {
if (!group) if (!group) {
{
return rclcpp::node::Node::SharedPtr(); return rclcpp::node::Node::SharedPtr();
} }
for (auto &weak_node : weak_nodes_) for (auto & weak_node : weak_nodes_) {
{
auto node = weak_node.lock(); auto node = weak_node.lock();
if (!node) if (!node) {
{
continue; continue;
} }
for (auto &weak_group : node->callback_groups_) for (auto & weak_group : node->callback_groups_) {
{
auto callback_group = weak_group.lock(); auto callback_group = weak_group.lock();
if (!callback_group) if (!callback_group) {
{
continue; continue;
} }
if (callback_group == group) if (callback_group == group) {
{
return node; return node;
} }
} }
@ -603,20 +535,15 @@ protected:
get_group_by_timer( get_group_by_timer(
rclcpp::timer::TimerBase::SharedPtr & timer) rclcpp::timer::TimerBase::SharedPtr & timer)
{ {
for (auto &weak_node : weak_nodes_) for (auto & weak_node : weak_nodes_) {
{
auto node = weak_node.lock(); auto node = weak_node.lock();
if (!node) if (!node) {
{
continue; continue;
} }
for (auto &weak_group : node->callback_groups_) for (auto & weak_group : node->callback_groups_) {
{
auto group = weak_group.lock(); auto group = weak_group.lock();
for (auto &t : group->timer_ptrs_) for (auto & t : group->timer_ptrs_) {
{ if (t == timer) {
if (t == timer)
{
return group; return group;
} }
} }
@ -628,22 +555,18 @@ protected:
void void
get_next_timer(std::shared_ptr<AnyExecutable> & any_exec) 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); auto timer = get_timer_by_handle(handle);
if (timer) if (timer) {
{
// Find the group for this handle and see if it can be serviced // Find the group for this handle and see if it can be serviced
auto group = get_group_by_timer(timer); auto group = get_group_by_timer(timer);
if (!group) if (!group) {
{
// Group was not found, meaning the timer is not valid... // Group was not found, meaning the timer is not valid...
// Remove it from the ready list and continue looking // Remove it from the ready list and continue looking
remove_guard_condition_handle_from_guard_condition_handles(handle); remove_guard_condition_handle_from_guard_condition_handles(handle);
continue; 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 // Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching // Leave it to be checked next time, but continue searching
continue; continue;
@ -664,20 +587,15 @@ protected:
get_group_by_subscription( get_group_by_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr & subscription) rclcpp::subscription::SubscriptionBase::SharedPtr & subscription)
{ {
for (auto &weak_node : weak_nodes_) for (auto & weak_node : weak_nodes_) {
{
auto node = weak_node.lock(); auto node = weak_node.lock();
if (!node) if (!node) {
{
continue; continue;
} }
for (auto &weak_group : node->callback_groups_) for (auto & weak_group : node->callback_groups_) {
{
auto group = weak_group.lock(); auto group = weak_group.lock();
for (auto &sub : group->subscription_ptrs_) for (auto & sub : group->subscription_ptrs_) {
{ if (sub == subscription) {
if (sub == subscription)
{
return group; return group;
} }
} }
@ -689,22 +607,18 @@ protected:
void void
get_next_subscription(std::shared_ptr<AnyExecutable> & any_exec) 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); auto subscription = get_subscription_by_handle(handle);
if (subscription) if (subscription) {
{
// Find the group for this handle and see if it can be serviced // Find the group for this handle and see if it can be serviced
auto group = get_group_by_subscription(subscription); auto group = get_group_by_subscription(subscription);
if (!group) if (!group) {
{
// Group was not found, meaning the subscription is not valid... // Group was not found, meaning the subscription is not valid...
// Remove it from the ready list and continue looking // Remove it from the ready list and continue looking
remove_subscriber_handle_from_subscriber_handles(handle); remove_subscriber_handle_from_subscriber_handles(handle);
continue; 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 // Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching // Leave it to be checked next time, but continue searching
continue; continue;
@ -725,20 +639,15 @@ protected:
get_group_by_service( get_group_by_service(
rclcpp::service::ServiceBase::SharedPtr & service) rclcpp::service::ServiceBase::SharedPtr & service)
{ {
for (auto &weak_node : weak_nodes_) for (auto & weak_node : weak_nodes_) {
{
auto node = weak_node.lock(); auto node = weak_node.lock();
if (!node) if (!node) {
{
continue; continue;
} }
for (auto &weak_group : node->callback_groups_) for (auto & weak_group : node->callback_groups_) {
{
auto group = weak_group.lock(); auto group = weak_group.lock();
for (auto &serv : group->service_ptrs_) for (auto & serv : group->service_ptrs_) {
{ if (serv == service) {
if (serv == service)
{
return group; return group;
} }
} }
@ -750,22 +659,18 @@ protected:
void void
get_next_service(std::shared_ptr<AnyExecutable> & any_exec) 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); auto service = get_service_by_handle(handle);
if (service) if (service) {
{
// Find the group for this handle and see if it can be serviced // Find the group for this handle and see if it can be serviced
auto group = get_group_by_service(service); auto group = get_group_by_service(service);
if (!group) if (!group) {
{
// Group was not found, meaning the service is not valid... // Group was not found, meaning the service is not valid...
// Remove it from the ready list and continue looking // Remove it from the ready list and continue looking
remove_service_handle_from_service_handles(handle); remove_service_handle_from_service_handles(handle);
continue; 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 // Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching // Leave it to be checked next time, but continue searching
continue; continue;
@ -786,20 +691,15 @@ protected:
get_group_by_client( get_group_by_client(
rclcpp::client::ClientBase::SharedPtr & client) rclcpp::client::ClientBase::SharedPtr & client)
{ {
for (auto &weak_node : weak_nodes_) for (auto & weak_node : weak_nodes_) {
{
auto node = weak_node.lock(); auto node = weak_node.lock();
if (!node) if (!node) {
{
continue; continue;
} }
for (auto &weak_group : node->callback_groups_) for (auto & weak_group : node->callback_groups_) {
{
auto group = weak_group.lock(); auto group = weak_group.lock();
for (auto &cli : group->client_ptrs_) for (auto & cli : group->client_ptrs_) {
{ if (cli == client) {
if (cli == client)
{
return group; return group;
} }
} }
@ -811,22 +711,18 @@ protected:
void void
get_next_client(std::shared_ptr<AnyExecutable> & any_exec) 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); auto client = get_client_by_handle(handle);
if (client) if (client) {
{
// Find the group for this handle and see if it can be serviced // Find the group for this handle and see if it can be serviced
auto group = get_group_by_client(client); auto group = get_group_by_client(client);
if (!group) if (!group) {
{
// Group was not found, meaning the service is not valid... // Group was not found, meaning the service is not valid...
// Remove it from the ready list and continue looking // Remove it from the ready list and continue looking
remove_client_handle_from_client_handles(handle); remove_client_handle_from_client_handles(handle);
continue; 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 // Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching // Leave it to be checked next time, but continue searching
continue; continue;
@ -849,26 +745,22 @@ protected:
std::shared_ptr<AnyExecutable> any_exec(new AnyExecutable()); std::shared_ptr<AnyExecutable> any_exec(new AnyExecutable());
// 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_exec);
if (any_exec->timer) if (any_exec->timer) {
{
return any_exec; return any_exec;
} }
// Check the subscriptions to see if there are any that are ready // Check the subscriptions to see if there are any that are ready
get_next_subscription(any_exec); get_next_subscription(any_exec);
if (any_exec->subscription) if (any_exec->subscription) {
{
return any_exec; return any_exec;
} }
// Check the services to see if there are any that are ready // Check the services to see if there are any that are ready
get_next_service(any_exec); get_next_service(any_exec);
if (any_exec->service) if (any_exec->service) {
{
return any_exec; return any_exec;
} }
// Check the clients to see if there are any that are ready // Check the clients to see if there are any that are ready
get_next_client(any_exec); get_next_client(any_exec);
if (any_exec->client) if (any_exec->client) {
{
return any_exec; return any_exec;
} }
// If there is neither a ready timer nor subscription, return a null ptr // If there is neither a ready timer nor subscription, return a null ptr
@ -883,8 +775,7 @@ protected:
// 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(); auto any_exec = get_next_ready_executable();
// If there are none // If there are none
if (!any_exec) if (!any_exec) {
{
// Wait for subscriptions or timers to work on // Wait for subscriptions or timers to work on
wait_for_work(nonblocking); wait_for_work(nonblocking);
// Try again // Try again
@ -892,15 +783,14 @@ protected:
} }
// 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 (any_exec) {
{
// 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->type_ == \ if (any_exec->callback_group->type_ == \
callback_group::CallbackGroupType::MutuallyExclusive) callback_group::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() == true); assert(any_exec->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
any_exec->callback_group->can_be_taken_from_.store(false); 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::multi_threaded_executor::MultiThreadedExecutor;
using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
} } // namespace executors
} } // namespace rclcpp
#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */ #endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */

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

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

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

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

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

@ -29,13 +29,19 @@
#include <rclcpp/timer.hpp> #include <rclcpp/timer.hpp>
// Forward declaration of ROS middleware class // Forward declaration of ROS middleware class
namespace rmw {struct rmw_node_t;} namespace rmw
{
struct rmw_node_t;
} // namespace rmw
namespace rclcpp namespace rclcpp
{ {
// Forward declaration for friend statement // Forward declaration for friend statement
namespace executor {class Executor;} namespace executor
{
class Executor;
} // namespace executor
namespace node namespace node
{ {
@ -47,6 +53,7 @@ namespace node
class Node class Node
{ {
friend class rclcpp::executor::Executor; friend class rclcpp::executor::Executor;
public: public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Node); RCLCPP_MAKE_SHARED_DEFINITIONS(Node);
@ -64,7 +71,8 @@ public:
create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
/* Create and return a Publisher. */ /* 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_publisher(std::string topic_name, size_t queue_size);
/* Create and return a Subscription. */ /* Create and return a Subscription. */
@ -105,7 +113,8 @@ public:
typename rclcpp::service::Service<ServiceT>::SharedPtr typename rclcpp::service::Service<ServiceT>::SharedPtr
create_service( create_service(
std::string service_name, std::string service_name,
std::function<void(const std::shared_ptr<typename ServiceT::Request> &, std::function<void(
const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response> &)> callback, std::shared_ptr<typename ServiceT::Response> &)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
@ -138,7 +147,9 @@ private:
rclcpp::node::Node::SharedPtr \ rclcpp::node::Node::SharedPtr \
create_node() \ 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_ #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) Node::group_in_node(callback_group::CallbackGroup::SharedPtr & group)
{ {
bool group_belongs_to_this_node = false; 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(); auto cur_group = weak_group.lock();
if (cur_group && cur_group == group) if (cur_group && (cur_group == group)) {
{
group_belongs_to_this_node = true; group_belongs_to_this_node = true;
} }
} }
@ -100,21 +98,18 @@ Node::create_subscription(
using namespace rclcpp::subscription; using namespace rclcpp::subscription;
auto sub = Subscription<MessageT>::make_shared(subscriber_handle, auto sub = Subscription<MessageT>::make_shared(
subscriber_handle,
topic_name, topic_name,
callback); callback);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub); auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
if (group) if (group) {
{ if (!group_in_node(group)) {
if (!group_in_node(group))
{
// TODO: use custom exception // TODO: use custom exception
throw std::runtime_error("Cannot create timer, group not in node."); throw std::runtime_error("Cannot create timer, group not in node.");
} }
group->add_subscription(sub_base_ptr); group->add_subscription(sub_base_ptr);
} } else {
else
{
default_callback_group_->add_subscription(sub_base_ptr); default_callback_group_->add_subscription(sub_base_ptr);
} }
number_of_subscriptions_++; number_of_subscriptions_++;
@ -122,22 +117,19 @@ Node::create_subscription(
} }
rclcpp::timer::WallTimer::SharedPtr 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::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group) rclcpp::callback_group::CallbackGroup::SharedPtr group)
{ {
auto timer = rclcpp::timer::WallTimer::make_shared(period, callback); auto timer = rclcpp::timer::WallTimer::make_shared(period, callback);
if (group) if (group) {
{ if (!group_in_node(group)) {
if (!group_in_node(group))
{
// TODO: use custom exception // TODO: use custom exception
throw std::runtime_error("Cannot create timer, group not in node."); throw std::runtime_error("Cannot create timer, group not in node.");
} }
group->add_timer(timer); group->add_timer(timer);
} } else {
else
{
default_callback_group_->add_timer(timer); default_callback_group_->add_timer(timer);
} }
number_of_timers_++; number_of_timers_++;
@ -171,21 +163,18 @@ Node::create_client(
using namespace rclcpp::client; using namespace rclcpp::client;
auto cli = Client<ServiceT>::make_shared(client_handle, auto cli = Client<ServiceT>::make_shared(
client_handle,
service_name); service_name);
auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli); auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
if (group) if (group) {
{ if (!group_in_node(group)) {
if (!group_in_node(group))
{
// TODO(esteve): use custom exception // TODO(esteve): use custom exception
throw std::runtime_error("Cannot create client, group not in node."); throw std::runtime_error("Cannot create client, group not in node.");
} }
group->add_client(cli_base_ptr); group->add_client(cli_base_ptr);
} } else {
else
{
default_callback_group_->add_client(cli_base_ptr); default_callback_group_->add_client(cli_base_ptr);
} }
number_of_clients_++; number_of_clients_++;
@ -210,22 +199,19 @@ Node::create_service(
using namespace rclcpp::service; using namespace rclcpp::service;
auto serv = Service<ServiceT>::make_shared(service_handle, auto serv = Service<ServiceT>::make_shared(
service_handle,
service_name, service_name,
callback); callback);
auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv); auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
if (group) if (group) {
{ if (!group_in_node(group)) {
if (!group_in_node(group))
{
// TODO: use custom exception // TODO: use custom exception
throw std::runtime_error("Cannot create service, group not in node."); throw std::runtime_error("Cannot create service, group not in node.");
} }
group->add_service(serv_base_ptr); group->add_service(serv_base_ptr);
} } else {
else
{
default_callback_group_->add_service(serv_base_ptr); default_callback_group_->add_service(serv_base_ptr);
} }
number_of_services_++; number_of_services_++;

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

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

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

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

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

@ -28,7 +28,10 @@ namespace rclcpp
{ {
// Forward declaration for friend statement // Forward declaration for friend statement
namespace executor {class Executor;} namespace executor
{
class Executor;
} // namespace executor
namespace service namespace service
{ {
@ -36,18 +39,19 @@ namespace service
class ServiceBase class ServiceBase
{ {
friend class rclcpp::executor::Executor; friend class rclcpp::executor::Executor;
public: public:
RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase); RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase);
ServiceBase(rmw_service_t * service_handle, ServiceBase(
rmw_service_t * service_handle,
const std::string service_name) const std::string service_name)
: service_handle_(service_handle), service_name_(service_name) : service_handle_(service_handle), service_name_(service_name)
{} {}
~ServiceBase() ~ServiceBase()
{ {
if (service_handle_ != nullptr) if (service_handle_ != nullptr) {
{
rmw_destroy_service(service_handle_); rmw_destroy_service(service_handle_);
service_handle_ = nullptr; service_handle_ = nullptr;
} }
@ -65,7 +69,8 @@ public:
virtual std::shared_ptr<void> create_request() = 0; virtual std::shared_ptr<void> create_request() = 0;
virtual std::shared_ptr<void> create_request_header() = 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; std::shared_ptr<void> & request) = 0;
private: private:
@ -85,7 +90,8 @@ public:
std::shared_ptr<typename ServiceT::Response> &)> CallbackType; std::shared_ptr<typename ServiceT::Response> &)> CallbackType;
RCLCPP_MAKE_SHARED_DEFINITIONS(Service); RCLCPP_MAKE_SHARED_DEFINITIONS(Service);
Service(rmw_service_t * service_handle, Service(
rmw_service_t * service_handle,
const std::string & service_name, const std::string & service_name,
CallbackType callback) CallbackType callback)
: ServiceBase(service_handle, service_name), callback_(callback) : ServiceBase(service_handle, service_name), callback_(callback)
@ -112,7 +118,8 @@ public:
send_response(typed_request_header, response); 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) std::shared_ptr<typename ServiceT::Response> & response)
{ {
rmw_send_response(get_service_handle(), req_id.get(), response.get()); 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 // Forward declaration is for friend statement in SubscriptionBase
namespace executor {class Executor;} namespace executor
{
class Executor;
} // namespace executor
namespace subscription namespace subscription
{ {
@ -35,6 +38,7 @@ namespace subscription
class SubscriptionBase class SubscriptionBase
{ {
friend class rclcpp::executor::Executor; friend class rclcpp::executor::Executor;
public: public:
RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase); RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase);
@ -67,7 +71,8 @@ public:
typedef std::function<void (const std::shared_ptr<MessageT> &)> CallbackType; typedef std::function<void (const std::shared_ptr<MessageT> &)> CallbackType;
RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription); RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription);
Subscription(rmw_subscription_t * subscription_handle, Subscription(
rmw_subscription_t * subscription_handle,
std::string & topic_name, std::string & topic_name,
CallbackType callback) CallbackType callback)
: SubscriptionBase(subscription_handle, topic_name), callback_(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 // Forward declaration is for friend statement in GenericTimer
namespace executor {class Executor;} namespace executor
{
class Executor;
} // namespace executor
namespace timer namespace timer
{ {
@ -40,6 +43,7 @@ typedef std::function<void()> CallbackType;
class TimerBase class TimerBase
{ {
friend class rclcpp::executor::Executor; friend class rclcpp::executor::Executor;
public: public:
RCLCPP_MAKE_SHARED_DEFINITIONS(TimerBase); RCLCPP_MAKE_SHARED_DEFINITIONS(TimerBase);
@ -72,6 +76,7 @@ template<class Clock = std::chrono::high_resolution_clock>
class GenericTimer : public TimerBase class GenericTimer : public TimerBase
{ {
friend class rclcpp::executor::Executor; friend class rclcpp::executor::Executor;
public: public:
RCLCPP_MAKE_SHARED_DEFINITIONS(GenericTimer); RCLCPP_MAKE_SHARED_DEFINITIONS(GenericTimer);
@ -89,11 +94,9 @@ public:
void void
run() run()
{ {
while (rclcpp::utilities::ok() && !this->canceled_) while (rclcpp::utilities::ok() && !this->canceled_) {
{
loop_rate_.sleep(); loop_rate_.sleep();
if (!rclcpp::utilities::ok()) if (!rclcpp::utilities::ok()) {
{
return; return;
} }
rmw_trigger_guard_condition(guard_condition_); rmw_trigger_guard_condition(guard_condition_);

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

@ -58,25 +58,23 @@ namespace
// TODO(wjwwood): remove // TODO(wjwwood): remove
std::cout << "signal_handler(" << signal_value << ")" << std::endl; std::cout << "signal_handler(" << signal_value << ")" << std::endl;
#ifdef HAS_SIGACTION #ifdef HAS_SIGACTION
if (old_action.sa_flags & SA_SIGINFO) if (old_action.sa_flags & SA_SIGINFO) {
{ if (old_action.sa_sigaction != NULL) {
if (old_action.sa_sigaction != NULL)
{
old_action.sa_sigaction(signal_value, siginfo, context); old_action.sa_sigaction(signal_value, siginfo, context);
} }
} } else {
else // *INDENT-OFF*
{ if (
if (old_action.sa_handler != NULL && // Is set old_action.sa_handler != NULL && // Is set
old_action.sa_handler != SIG_DFL && // Is not default 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
// *INDENT-ON*
{ {
old_action.sa_handler(signal_value); old_action.sa_handler(signal_value);
} }
} }
#else #else
if (old_signal_handler) if (old_signal_handler) {
{
old_signal_handler(signal_value); old_signal_handler(signal_value);
} }
#endif #endif
@ -84,7 +82,7 @@ namespace
rmw_trigger_guard_condition(g_sigint_guard_cond_handle); rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
g_interrupt_condition_variable.notify_all(); g_interrupt_condition_variable.notify_all();
} }
} } // namespace
namespace rclcpp namespace rclcpp
{ {
@ -112,11 +110,12 @@ init(int argc, char *argv[])
if (::old_signal_handler == SIG_ERR) if (::old_signal_handler == SIG_ERR)
#endif #endif
{ {
// *INDENT-OFF*
throw std::runtime_error( throw std::runtime_error(
std::string("Failed to set SIGINT signal handler: (" + std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") +
std::to_string(errno) + ")") +
// TODO(wjwwood): use strerror_r on POSIX and strerror_s on Windows. // TODO(wjwwood): use strerror_r on POSIX and strerror_s on Windows.
std::strerror(errno)); std::strerror(errno));
// *INDENT-ON*
} }
} }