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

24
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);
@ -52,25 +62,25 @@ private:
void void
add_subscription( add_subscription(
const subscription::SubscriptionBase::SharedPtr &subscription_ptr) const subscription::SubscriptionBase::SharedPtr & subscription_ptr)
{ {
subscription_ptrs_.push_back(subscription_ptr); subscription_ptrs_.push_back(subscription_ptr);
} }
void void
add_timer(const timer::TimerBase::SharedPtr &timer_ptr) add_timer(const timer::TimerBase::SharedPtr & timer_ptr)
{ {
timer_ptrs_.push_back(timer_ptr); timer_ptrs_.push_back(timer_ptr);
} }
void void
add_service(const service::ServiceBase::SharedPtr &service_ptr) add_service(const service::ServiceBase::SharedPtr & service_ptr)
{ {
service_ptrs_.push_back(service_ptr); service_ptrs_.push_back(service_ptr);
} }
void void
add_client(const client::ClientBase::SharedPtr &client_ptr) add_client(const client::ClientBase::SharedPtr & client_ptr)
{ {
client_ptrs_.push_back(client_ptr); client_ptrs_.push_back(client_ptr);
} }

31
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);
@ -85,12 +87,11 @@ public:
typedef std::shared_ptr<Promise> SharedPromise; typedef std::shared_ptr<Promise> SharedPromise;
typedef std::shared_future<typename ServiceT::Response::Ptr> SharedFuture; typedef std::shared_future<typename ServiceT::Response::Ptr> SharedFuture;
typedef std::function<void(SharedFuture)> CallbackType; typedef std::function<void (SharedFuture)> CallbackType;
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)
{} {}
@ -106,7 +107,7 @@ public:
return std::shared_ptr<void>(new rmw_request_id_t); return std::shared_ptr<void>(new rmw_request_id_t);
} }
void handle_response(std::shared_ptr<void> &request_header, std::shared_ptr<void> &response) void handle_response(std::shared_ptr<void> & request_header, std::shared_ptr<void> & response)
{ {
auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header); auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response); auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
@ -121,13 +122,13 @@ public:
} }
SharedFuture async_send_request( SharedFuture async_send_request(
typename ServiceT::Request::Ptr &request) typename ServiceT::Request::Ptr & request)
{ {
return async_send_request(request, [] (SharedFuture f) { }); return async_send_request(request, [](SharedFuture f) {});
} }
SharedFuture async_send_request( SharedFuture async_send_request(
typename ServiceT::Request::Ptr &request, typename ServiceT::Request::Ptr & request,
CallbackType cb) CallbackType cb)
{ {
int64_t sequence_number; int64_t sequence_number;
@ -143,7 +144,7 @@ public:
private: private:
RCLCPP_DISABLE_COPY(Client); RCLCPP_DISABLE_COPY(Client);
std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture> > pending_requests_; std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
}; };
} /* namespace client */ } /* namespace client */

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_ */

438
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_);
} }
@ -52,17 +53,14 @@ public:
virtual void spin() = 0; virtual void spin() = 0;
virtual void virtual void
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);
@ -71,31 +69,30 @@ public:
} }
virtual void virtual void
remove_node(rclcpp::node::Node::SharedPtr &node_ptr) remove_node(rclcpp::node::Node::SharedPtr & node_ptr)
{ {
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(
[&](std::weak_ptr<rclcpp::node::Node> &i) weak_nodes_.begin(), weak_nodes_.end(),
[&](std::weak_ptr<rclcpp::node::Node> & i)
{ {
bool matched = (i.lock() == node_ptr); bool matched = (i.lock() == node_ptr);
node_removed |= matched; node_removed |= matched;
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_);
} }
} }
void spin_node_some(rclcpp::node::Node::SharedPtr &node) void spin_node_some(rclcpp::node::Node::SharedPtr & node)
{ {
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;
@ -116,26 +115,21 @@ 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
@ -147,17 +141,14 @@ protected:
static void static void
execute_subscription( execute_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr &subscription) rclcpp::subscription::SubscriptionBase::SharedPtr & subscription)
{ {
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;
@ -166,28 +157,26 @@ protected:
static void static void
execute_timer( execute_timer(
rclcpp::timer::TimerBase::SharedPtr &timer) rclcpp::timer::TimerBase::SharedPtr & timer)
{ {
timer->callback_(); timer->callback_();
} }
static void static void
execute_service( execute_service(
rclcpp::service::ServiceBase::SharedPtr &service) rclcpp::service::ServiceBase::SharedPtr & service)
{ {
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;
@ -196,21 +185,19 @@ protected:
static void static void
execute_client( execute_client(
rclcpp::client::ClientBase::SharedPtr &client) rclcpp::client::ClientBase::SharedPtr & client)
{ {
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;
} }
} }
@ -546,52 +484,46 @@ protected:
void void
remove_subscriber_handle_from_subscriber_handles(void *handle) remove_subscriber_handle_from_subscriber_handles(void * handle)
{ {
subscriber_handles_.remove(handle); subscriber_handles_.remove(handle);
} }
void void
remove_guard_condition_handle_from_guard_condition_handles(void *handle) remove_guard_condition_handle_from_guard_condition_handles(void * handle)
{ {
guard_condition_handles_.remove(handle); guard_condition_handles_.remove(handle);
} }
void void
remove_service_handle_from_service_handles(void *handle) remove_service_handle_from_service_handles(void * handle)
{ {
service_handles_.remove(handle); service_handles_.remove(handle);
} }
void void
remove_client_handle_from_client_handles(void *handle) remove_client_handle_from_client_handles(void * handle)
{ {
client_handles_.remove(handle); client_handles_.remove(handle);
} }
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;
} }
} }
@ -601,22 +533,17 @@ protected:
rclcpp::callback_group::CallbackGroup::SharedPtr rclcpp::callback_group::CallbackGroup::SharedPtr
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;
} }
} }
@ -626,24 +553,20 @@ 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;
@ -662,22 +585,17 @@ protected:
rclcpp::callback_group::CallbackGroup::SharedPtr rclcpp::callback_group::CallbackGroup::SharedPtr
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;
} }
} }
@ -687,24 +605,20 @@ 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;
@ -723,22 +637,17 @@ protected:
rclcpp::callback_group::CallbackGroup::SharedPtr rclcpp::callback_group::CallbackGroup::SharedPtr
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;
} }
} }
@ -748,24 +657,20 @@ 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;
@ -784,22 +689,17 @@ protected:
rclcpp::callback_group::CallbackGroup::SharedPtr rclcpp::callback_group::CallbackGroup::SharedPtr
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;
} }
} }
@ -809,24 +709,20 @@ 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
@ -877,14 +769,13 @@ protected:
} }
std::shared_ptr<AnyExecutable> std::shared_ptr<AnyExecutable>
get_next_executable(bool nonblocking=false) get_next_executable(bool nonblocking = false)
{ {
// Check to see if there are any subscriptions or timers needing service // Check to see if there are any subscriptions or timers needing service
// TODO(wjwwood): improve run to run efficiency of this function // TODO(wjwwood): improve run to run efficiency of this function
auto any_exec = get_next_ready_executable(); 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);
} }
@ -914,13 +804,13 @@ private:
RCLCPP_DISABLE_COPY(Executor); RCLCPP_DISABLE_COPY(Executor);
std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_; std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
typedef std::list<void*> SubscriberHandles; typedef std::list<void *> SubscriberHandles;
SubscriberHandles subscriber_handles_; SubscriberHandles subscriber_handles_;
typedef std::list<void*> GuardConditionHandles; typedef std::list<void *> GuardConditionHandles;
GuardConditionHandles guard_condition_handles_; GuardConditionHandles guard_condition_handles_;
typedef std::list<void*> ServiceHandles; typedef std::list<void *> ServiceHandles;
ServiceHandles service_handles_; ServiceHandles service_handles_;
typedef std::list<void*> ClientHandles; typedef std::list<void *> ClientHandles;
ClientHandles client_handles_; ClientHandles client_handles_;
}; };

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);
} }

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

@ -20,8 +20,8 @@
* Use in the private section of the class. * Use in the private section of the class.
*/ */
#define RCLCPP_DISABLE_COPY(Class) \ #define RCLCPP_DISABLE_COPY(Class) \
Class(const Class&) = delete; \ Class(const Class &) = delete; \
Class& operator=(const Class&) = delete; Class & operator=(const Class &) = delete;
/* Defines a make_shared static function on the class using perfect forwarding. /* Defines a make_shared static function on the class using perfect forwarding.
* *
@ -31,11 +31,11 @@
#define RCLCPP_MAKE_SHARED_DEFINITIONS(Class) \ #define RCLCPP_MAKE_SHARED_DEFINITIONS(Class) \
typedef std::shared_ptr<Class> SharedPtr; \ typedef std::shared_ptr<Class> SharedPtr; \
\ \
template<typename...Args> \ template<typename ... Args> \
static std::shared_ptr<Class> \ static std::shared_ptr<Class> \
make_shared(Args &&...args) \ make_shared(Args && ... args) \
{ \ { \
return std::make_shared<Class>(std::forward<Args>(args)...); \ return std::make_shared<Class>(std::forward<Args>(args) ...); \
} }
#define RCLCPP_INFO(Args) std::cout << Args << std::endl; #define RCLCPP_INFO(Args) std::cout << Args << std::endl;

47
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. */
@ -74,46 +82,47 @@ public:
std::string topic_name, std::string topic_name,
size_t queue_size, size_t queue_size,
std::function<void(const std::shared_ptr<MessageT> &)> callback, std::function<void(const std::shared_ptr<MessageT> &)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
/* Create a timer. */ /* Create a timer. */
rclcpp::timer::WallTimer::SharedPtr rclcpp::timer::WallTimer::SharedPtr
create_wall_timer( create_wall_timer(
std::chrono::nanoseconds period, std::chrono::nanoseconds period,
rclcpp::timer::CallbackType callback, rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
rclcpp::timer::WallTimer::SharedPtr rclcpp::timer::WallTimer::SharedPtr
create_wall_timer( create_wall_timer(
std::chrono::duration<long double, std::nano> period, std::chrono::duration<long double, std::nano> period,
rclcpp::timer::CallbackType callback, rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
typedef rclcpp::callback_group::CallbackGroup CallbackGroup; typedef rclcpp::callback_group::CallbackGroup CallbackGroup;
typedef std::weak_ptr<CallbackGroup> CallbackGroupWeakPtr; typedef std::weak_ptr<CallbackGroup> CallbackGroupWeakPtr;
typedef std::list<CallbackGroupWeakPtr> CallbackGroupWeakPtrList; typedef std::list<CallbackGroupWeakPtr> CallbackGroupWeakPtrList;
/* Create and return a Client. */ /* Create and return a Client. */
template <typename ServiceT> template<typename ServiceT>
typename rclcpp::client::Client<ServiceT>::SharedPtr typename rclcpp::client::Client<ServiceT>::SharedPtr
create_client( create_client(
std::string service_name, std::string service_name,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
/* Create and return a Service. */ /* Create and return a Service. */
template <typename ServiceT> template<typename ServiceT>
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(
std::shared_ptr<typename ServiceT::Response>&)> callback, const std::shared_ptr<typename ServiceT::Request> &,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); std::shared_ptr<typename ServiceT::Response> &)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
private: private:
RCLCPP_DISABLE_COPY(Node); RCLCPP_DISABLE_COPY(Node);
bool bool
group_in_node(callback_group::CallbackGroup::SharedPtr &group); group_in_node(callback_group::CallbackGroup::SharedPtr & group);
std::string name_; std::string name_;
@ -135,11 +144,13 @@ private:
} /* namespace rclcpp */ } /* namespace rclcpp */
#define RCLCPP_REGISTER_NODE(Class) RMW_EXPORT \ #define RCLCPP_REGISTER_NODE(Class) RMW_EXPORT \
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_
// Template implementations // Template implementations

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

@ -34,11 +34,11 @@ using namespace rclcpp::node;
using rclcpp::contexts::default_context::DefaultContext; using rclcpp::contexts::default_context::DefaultContext;
Node::Node(std::string node_name) Node::Node(std::string node_name)
: Node(node_name, DefaultContext::make_shared()) : Node(node_name, DefaultContext::make_shared())
{} {}
Node::Node(std::string node_name, context::Context::SharedPtr context) Node::Node(std::string node_name, context::Context::SharedPtr context)
: name_(node_name), context_(context), : name_(node_name), context_(context),
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0) number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0)
{ {
node_handle_ = rmw_create_node(name_.c_str()); node_handle_ = rmw_create_node(name_.c_str());
@ -71,14 +71,12 @@ Node::create_publisher(std::string topic_name, size_t queue_size)
} }
bool 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_++;
@ -193,12 +182,12 @@ Node::create_client(
return cli; return cli;
} }
template <typename ServiceT> template<typename ServiceT>
typename service::Service<ServiceT>::SharedPtr typename service::Service<ServiceT>::SharedPtr
Node::create_service( Node::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) rclcpp::callback_group::CallbackGroup::SharedPtr group)
{ {
using rosidl_generator_cpp::get_service_type_support_handle; using rosidl_generator_cpp::get_service_type_support_handle;
@ -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_++;

10
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,8 +42,9 @@ public:
: publisher_handle_(publisher_handle) : publisher_handle_(publisher_handle)
{} {}
template<typename MessageT> void template<typename MessageT>
publish(std::shared_ptr<MessageT> &msg) void
publish(std::shared_ptr<MessageT> & msg)
{ {
rmw_publish(publisher_handle_, msg.get()); rmw_publish(publisher_handle_, msg.get());
} }

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

@ -49,7 +49,7 @@ public:
GenericRate(double rate) GenericRate(double rate)
: GenericRate<Clock>( : GenericRate<Clock>(
duration_cast<nanoseconds>(duration<double>(1.0/rate))) duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
{} {}
GenericRate(std::chrono::nanoseconds period) GenericRate(std::chrono::nanoseconds period)
: period_(period), last_interval_(Clock::now()) : period_(period), last_interval_(Clock::now())
@ -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_ */

12
rclcpp/include/rclcpp/rclcpp.hpp
View file

@ -26,23 +26,23 @@
namespace rclcpp namespace rclcpp
{ {
const std::chrono::seconds operator "" _s(unsigned long long s) const std::chrono::seconds operator"" _s(unsigned long long s)
{ {
return std::chrono::seconds(s); return std::chrono::seconds(s);
} }
const std::chrono::nanoseconds operator "" _s(long double s) const std::chrono::nanoseconds operator"" _s(long double s)
{ {
return std::chrono::duration_cast<std::chrono::nanoseconds>( return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<long double>(s)); std::chrono::duration<long double>(s));
} }
const std::chrono::nanoseconds const std::chrono::nanoseconds
operator "" _ns(unsigned long long ns) operator"" _ns(unsigned long long ns)
{ {
return std::chrono::nanoseconds(ns); return std::chrono::nanoseconds(ns);
} }
const std::chrono::nanoseconds const std::chrono::nanoseconds
operator "" _ns(long double ns) operator"" _ns(long double ns)
{ {
return std::chrono::duration_cast<std::chrono::nanoseconds>( return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<long double, std::nano>(ns)); std::chrono::duration<long double, std::nano>(ns));
@ -63,13 +63,13 @@ using rclcpp::utilities::shutdown;
using rclcpp::utilities::init; using rclcpp::utilities::init;
using rclcpp::utilities::sleep_for; using rclcpp::utilities::sleep_for;
void spin_some(Node::SharedPtr &node_ptr) void spin_some(Node::SharedPtr & node_ptr)
{ {
rclcpp::executors::SingleThreadedExecutor executor; rclcpp::executors::SingleThreadedExecutor executor;
executor.spin_node_some(node_ptr); executor.spin_node_some(node_ptr);
} }
void spin(Node::SharedPtr &node_ptr) void spin(Node::SharedPtr & node_ptr)
{ {
rclcpp::executors::SingleThreadedExecutor executor; rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node_ptr); executor.add_node(node_ptr);

33
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,8 +69,9 @@ 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) = 0; std::shared_ptr<void> & request_header,
std::shared_ptr<void> & request) = 0;
private: private:
RCLCPP_DISABLE_COPY(ServiceBase); RCLCPP_DISABLE_COPY(ServiceBase);
@ -81,12 +86,13 @@ class Service : public ServiceBase
{ {
public: public:
typedef std::function< typedef std::function<
void(const std::shared_ptr<typename ServiceT::Request> &, void (const std::shared_ptr<typename ServiceT::Request> &,
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(
const std::string &service_name, rmw_service_t * service_handle,
const std::string & service_name,
CallbackType callback) CallbackType callback)
: ServiceBase(service_handle, service_name), callback_(callback) : ServiceBase(service_handle, service_name), callback_(callback)
{} {}
@ -103,7 +109,7 @@ public:
return std::shared_ptr<void>(new rmw_request_id_t); return std::shared_ptr<void>(new rmw_request_id_t);
} }
void handle_request(std::shared_ptr<void> &request_header, std::shared_ptr<void> &request) void handle_request(std::shared_ptr<void> & request_header, std::shared_ptr<void> & request)
{ {
auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request); auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header); auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
@ -112,8 +118,9 @@ 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<typename ServiceT::Response> &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()); rmw_send_response(get_service_handle(), req_id.get(), response.get());
} }

19
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,12 +38,13 @@ 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);
SubscriptionBase( SubscriptionBase(
rmw_subscription_t * subscription_handle, rmw_subscription_t * subscription_handle,
std::string &topic_name) std::string & topic_name)
: subscription_handle_(subscription_handle), topic_name_(topic_name) : subscription_handle_(subscription_handle), topic_name_(topic_name)
{} {}
@ -50,7 +54,7 @@ public:
} }
virtual std::shared_ptr<void> create_message() = 0; virtual std::shared_ptr<void> create_message() = 0;
virtual void handle_message(std::shared_ptr<void> &message) = 0; virtual void handle_message(std::shared_ptr<void> & message) = 0;
private: private:
RCLCPP_DISABLE_COPY(SubscriptionBase); RCLCPP_DISABLE_COPY(SubscriptionBase);
@ -64,11 +68,12 @@ template<typename MessageT>
class Subscription : public SubscriptionBase class Subscription : public SubscriptionBase
{ {
public: 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(
std::string &topic_name, rmw_subscription_t * subscription_handle,
std::string & topic_name,
CallbackType callback) CallbackType callback)
: SubscriptionBase(subscription_handle, topic_name), callback_(callback) : SubscriptionBase(subscription_handle, topic_name), callback_(callback)
{} {}
@ -78,7 +83,7 @@ public:
return std::shared_ptr<void>(new MessageT()); return std::shared_ptr<void>(new MessageT());
} }
void handle_message(std::shared_ptr<void> &message) void handle_message(std::shared_ptr<void> & message)
{ {
auto typed_message = std::static_pointer_cast<MessageT>(message); auto typed_message = std::static_pointer_cast<MessageT>(message);
callback_(typed_message); callback_(typed_message);

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

@ -30,16 +30,20 @@ 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
{ {
typedef std::function<void()> CallbackType; 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_);

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

@ -36,55 +36,53 @@
namespace namespace
{ {
volatile sig_atomic_t g_signal_status = 0; volatile sig_atomic_t g_signal_status = 0;
rmw_guard_condition_t * g_sigint_guard_cond_handle = \ rmw_guard_condition_t * g_sigint_guard_cond_handle = \
rmw_create_guard_condition(); rmw_create_guard_condition();
std::condition_variable g_interrupt_condition_variable; std::condition_variable g_interrupt_condition_variable;
std::mutex g_interrupt_mutex; std::mutex g_interrupt_mutex;
#ifdef HAS_SIGACTION #ifdef HAS_SIGACTION
struct sigaction old_action; struct sigaction old_action;
#else #else
void (*old_signal_handler)(int) = 0; void (* old_signal_handler)(int) = 0;
#endif #endif
void void
#ifdef HAS_SIGACTION #ifdef HAS_SIGACTION
signal_handler(int signal_value, siginfo_t *siginfo, void *context) signal_handler(int signal_value, siginfo_t * siginfo, void * context)
#else #else
signal_handler(int signal_value) signal_handler(int signal_value)
#endif #endif
{ {
// 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
g_signal_status = signal_value; g_signal_status = signal_value;
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
{ {
@ -95,7 +93,7 @@ namespace utilities
{ {
void void
init(int argc, char *argv[]) init(int argc, char * argv[])
{ {
// TODO(wjwwood): Handle rmw_ret_t's value. // TODO(wjwwood): Handle rmw_ret_t's value.
rmw_init(); rmw_init();
@ -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*
} }
} }
@ -142,7 +141,7 @@ get_global_sigint_guard_condition()
} }
bool bool
sleep_for(const std::chrono::nanoseconds& nanoseconds) sleep_for(const std::chrono::nanoseconds & nanoseconds)
{ {
// TODO: determine if posix's nanosleep(2) is more efficient here // TODO: determine if posix's nanosleep(2) is more efficient here
std::unique_lock<std::mutex> lock(::g_interrupt_mutex); std::unique_lock<std::mutex> lock(::g_interrupt_mutex);

2
rclcpp/src/node_main.cpp
View file

@ -17,7 +17,7 @@
// This forward declaration is implemented by the RCLCPP_REGISTER_NODE macro // This forward declaration is implemented by the RCLCPP_REGISTER_NODE macro
RMW_IMPORT rclcpp::Node::SharedPtr create_node(); RMW_IMPORT rclcpp::Node::SharedPtr create_node();
int main(int argc, char **argv) int main(int argc, char ** argv)
{ {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor executor; rclcpp::executors::SingleThreadedExecutor executor;