diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 455783b60e..e4eb0c914f 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -28,23 +28,33 @@ namespace rclcpp { // Forward declarations for friend statement in class CallbackGroup -namespace node {class Node;} -namespace executor {class Executor;} +namespace node +{ +class Node; +} // namespace node +namespace executor +{ +class Executor; +} // namespace executor namespace callback_group { -enum class CallbackGroupType {MutuallyExclusive, Reentrant}; +enum class CallbackGroupType { + MutuallyExclusive, + Reentrant +}; class CallbackGroup { friend class rclcpp::node::Node; friend class rclcpp::executor::Executor; + public: RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup); CallbackGroup(CallbackGroupType group_type) - : type_(group_type), can_be_taken_from_(true) + : type_(group_type), can_be_taken_from_(true) {} private: @@ -52,25 +62,25 @@ class CallbackGroup void add_subscription( - const subscription::SubscriptionBase::SharedPtr &subscription_ptr) + const subscription::SubscriptionBase::SharedPtr & subscription_ptr) { subscription_ptrs_.push_back(subscription_ptr); } void - add_timer(const timer::TimerBase::SharedPtr &timer_ptr) + add_timer(const timer::TimerBase::SharedPtr & timer_ptr) { timer_ptrs_.push_back(timer_ptr); } void - add_service(const service::ServiceBase::SharedPtr &service_ptr) + add_service(const service::ServiceBase::SharedPtr & service_ptr) { service_ptrs_.push_back(service_ptr); } void - add_client(const client::ClientBase::SharedPtr &client_ptr) + add_client(const client::ClientBase::SharedPtr & client_ptr) { client_ptrs_.push_back(client_ptr); } diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 3368935897..3e532f3982 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -29,7 +29,10 @@ namespace rclcpp { // Forward declaration for friend statement -namespace executor {class Executor;} +namespace executor +{ +class Executor; +} // namespace executor namespace client { @@ -37,18 +40,17 @@ namespace client class ClientBase { friend class rclcpp::executor::Executor; + public: RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase); - ClientBase(rmw_client_t * client_handle, - const std::string &service_name) + ClientBase(rmw_client_t * client_handle, const std::string & service_name) : client_handle_(client_handle), service_name_(service_name) {} ~ClientBase() { - if (client_handle_ != nullptr) - { + if (client_handle_ != nullptr) { rmw_destroy_client(client_handle_); client_handle_ = nullptr; } @@ -66,8 +68,8 @@ class ClientBase virtual std::shared_ptr create_response() = 0; virtual std::shared_ptr create_request_header() = 0; - virtual void handle_response(std::shared_ptr &request_header, - std::shared_ptr &response) = 0; + virtual void handle_response( + std::shared_ptr & request_header, std::shared_ptr & response) = 0; private: RCLCPP_DISABLE_COPY(ClientBase); @@ -85,12 +87,11 @@ class Client : public ClientBase typedef std::shared_ptr SharedPromise; typedef std::shared_future SharedFuture; - typedef std::function CallbackType; + typedef std::function CallbackType; RCLCPP_MAKE_SHARED_DEFINITIONS(Client); - Client(rmw_client_t * client_handle, - const std::string& service_name) + Client(rmw_client_t * client_handle, const std::string & service_name) : ClientBase(client_handle, service_name) {} @@ -106,7 +107,7 @@ class Client : public ClientBase return std::shared_ptr(new rmw_request_id_t); } - void handle_response(std::shared_ptr &request_header, std::shared_ptr &response) + void handle_response(std::shared_ptr & request_header, std::shared_ptr & response) { auto typed_request_header = std::static_pointer_cast(request_header); auto typed_response = std::static_pointer_cast(response); @@ -121,13 +122,13 @@ class Client : public ClientBase } 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( - typename ServiceT::Request::Ptr &request, + typename ServiceT::Request::Ptr & request, CallbackType cb) { int64_t sequence_number; @@ -143,7 +144,7 @@ class Client : public ClientBase private: RCLCPP_DISABLE_COPY(Client); - std::map > pending_requests_; + std::map> pending_requests_; }; } /* namespace client */ diff --git a/rclcpp/include/rclcpp/contexts/default_context.hpp b/rclcpp/include/rclcpp/contexts/default_context.hpp index d91102f4ba..0ccceb6018 100644 --- a/rclcpp/include/rclcpp/contexts/default_context.hpp +++ b/rclcpp/include/rclcpp/contexts/default_context.hpp @@ -33,8 +33,8 @@ class DefaultContext : public rclcpp::context::Context }; -} -} -} +} // namespace default_context +} // namespace contexts +} // namespace rclcpp #endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */ diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 8b5f1aecb4..4a99f58978 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -38,12 +38,13 @@ class Executor public: RCLCPP_MAKE_SHARED_DEFINITIONS(Executor); - Executor() : interrupt_guard_condition_(rmw_create_guard_condition()) {} + Executor() + : interrupt_guard_condition_(rmw_create_guard_condition()) + {} virtual ~Executor() { - if (interrupt_guard_condition_ != nullptr) - { + if (interrupt_guard_condition_ != nullptr) { // TODO(wjwwood): Check ret code. rmw_destroy_guard_condition(interrupt_guard_condition_); } @@ -52,17 +53,14 @@ class Executor virtual void spin() = 0; 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 - for (auto &weak_node : weak_nodes_) - { + for (auto & weak_node : weak_nodes_) { auto node = weak_node.lock(); - if (node == node_ptr) - { + if (node == node_ptr) { // TODO: Use a different error here? - throw std::runtime_error( - "Cannot add node to executor, node already added."); + throw std::runtime_error("Cannot add node to executor, node already added."); } } weak_nodes_.push_back(node_ptr); @@ -71,31 +69,30 @@ class Executor } virtual void - remove_node(rclcpp::node::Node::SharedPtr &node_ptr) + remove_node(rclcpp::node::Node::SharedPtr & node_ptr) { bool node_removed = false; weak_nodes_.erase( - std::remove_if(weak_nodes_.begin(), weak_nodes_.end(), - [&](std::weak_ptr &i) + std::remove_if( + weak_nodes_.begin(), weak_nodes_.end(), + [&](std::weak_ptr & i) { bool matched = (i.lock() == node_ptr); node_removed |= matched; return matched; })); // If the node was matched and removed, interrupt waiting - if (node_removed) - { + if (node_removed) { 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); // non-blocking = true std::shared_ptr any_exec; - while ((any_exec = get_next_executable(true))) - { + while ((any_exec = get_next_executable(true))) { execute_any_executable(any_exec); } this->remove_node(node); @@ -104,7 +101,9 @@ class Executor protected: 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 rclcpp::subscription::SubscriptionBase::SharedPtr subscription; rclcpp::timer::TimerBase::SharedPtr timer; @@ -116,26 +115,21 @@ class Executor }; void - execute_any_executable(std::shared_ptr &any_exec) + execute_any_executable(std::shared_ptr & any_exec) { - if (!any_exec) - { + if (!any_exec) { return; } - if (any_exec->timer) - { + if (any_exec->timer) { execute_timer(any_exec->timer); } - if (any_exec->subscription) - { + if (any_exec->subscription) { execute_subscription(any_exec->subscription); } - if (any_exec->service) - { + if (any_exec->service) { execute_service(any_exec->service); } - if (any_exec->client) - { + if (any_exec->client) { execute_client(any_exec->client); } // Reset the callback_group, regardless of type @@ -147,17 +141,14 @@ class Executor static void execute_subscription( - rclcpp::subscription::SubscriptionBase::SharedPtr &subscription) + rclcpp::subscription::SubscriptionBase::SharedPtr & subscription) { std::shared_ptr message = subscription->create_message(); bool taken = false; rmw_take(subscription->subscription_handle_, message.get(), &taken); - if (taken) - { + if (taken) { subscription->handle_message(message); - } - else - { + } else { std::cout << "[rclcpp::error] take failed for subscription on topic: " << subscription->get_topic_name() << std::endl; @@ -166,28 +157,26 @@ class Executor static void execute_timer( - rclcpp::timer::TimerBase::SharedPtr &timer) + rclcpp::timer::TimerBase::SharedPtr & timer) { timer->callback_(); } static void execute_service( - rclcpp::service::ServiceBase::SharedPtr &service) + rclcpp::service::ServiceBase::SharedPtr & service) { std::shared_ptr request_header = service->create_request_header(); std::shared_ptr request = service->create_request(); bool taken = false; - rmw_take_request(service->service_handle_, - request_header.get(), - request.get(), - &taken); - if (taken) - { + rmw_take_request( + service->service_handle_, + request_header.get(), + request.get(), + &taken); + if (taken) { service->handle_request(request_header, request); - } - else - { + } else { std::cout << "[rclcpp::error] take failed for service on service: " << service->get_service_name() << std::endl; @@ -196,21 +185,19 @@ class Executor static void execute_client( - rclcpp::client::ClientBase::SharedPtr &client) + rclcpp::client::ClientBase::SharedPtr & client) { std::shared_ptr request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); bool taken = false; - rmw_take_response(client->client_handle_, - request_header.get(), - response.get(), - &taken); - if (taken) - { + rmw_take_response( + client->client_handle_, + request_header.get(), + response.get(), + &taken); + if (taken) { client->handle_response(request_header, response); - } - else - { + } else { std::cout << "[rclcpp::error] take failed for service on client" << std::endl; } } @@ -226,47 +213,39 @@ class Executor std::vector timers; std::vector services; std::vector clients; - for (auto &weak_node : weak_nodes_) - { + for (auto & weak_node : weak_nodes_) { auto node = weak_node.lock(); - if (!node) - { + if (!node) { has_invalid_weak_nodes = false; continue; } - for (auto &weak_group : node->callback_groups_) - { + for (auto & weak_group : node->callback_groups_) { auto group = weak_group.lock(); - if (!group || group->can_be_taken_from_.load() == false) - { + if (!group || !group->can_be_taken_from_.load()) { continue; } - for (auto &subscription : group->subscription_ptrs_) - { + for (auto & subscription : group->subscription_ptrs_) { subs.push_back(subscription); } - for (auto &timer : group->timer_ptrs_) - { + for (auto & timer : group->timer_ptrs_) { timers.push_back(timer); } - for (auto &service : group->service_ptrs_) - { + for (auto & service : group->service_ptrs_) { services.push_back(service); } - for (auto &client : group->client_ptrs_) - { + for (auto & client : group->client_ptrs_) { clients.push_back(client); } } } // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_nodes) - { - weak_nodes_.erase(remove_if(weak_nodes_.begin(), weak_nodes_.end(), - [](std::weak_ptr i) - { - return i.expired(); - })); + if (has_invalid_weak_nodes) { + weak_nodes_.erase( + remove_if(weak_nodes_.begin(), weak_nodes_.end(), + [](std::weak_ptr i) + { + return i.expired(); + })); } // Use the number of subscriptions to allocate memory in the handles unsigned long number_of_subscriptions = subs.size(); @@ -275,15 +254,13 @@ class Executor // TODO(wjwwood): Avoid redundant malloc's subscriber_handles.subscribers = static_cast( std::malloc(sizeof(void *) * number_of_subscriptions)); - if (subscriber_handles.subscribers == NULL) - { + if (subscriber_handles.subscribers == NULL) { // TODO(wjwwood): Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for subscriber pointers."); } // Then fill the SubscriberHandles with ready subscriptions size_t subscriber_handle_index = 0; - for (auto &subscription : subs) - { + for (auto & subscription : subs) { subscriber_handles.subscribers[subscriber_handle_index] = \ subscription->subscription_handle_->data; subscriber_handle_index += 1; @@ -296,15 +273,13 @@ class Executor // TODO(esteve): Avoid redundant malloc's service_handles.services = static_cast( std::malloc(sizeof(void *) * number_of_services)); - if (service_handles.services == NULL) - { + if (service_handles.services == NULL) { // TODO(esteve): Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for service pointers."); } // Then fill the ServiceHandles with ready services size_t service_handle_index = 0; - for (auto &service : services) - { + for (auto & service : services) { service_handles.services[service_handle_index] = \ service->service_handle_->data; service_handle_index += 1; @@ -317,15 +292,13 @@ class Executor // TODO: Avoid redundant malloc's client_handles.clients = static_cast( std::malloc(sizeof(void *) * number_of_clients)); - if (client_handles.clients == NULL) - { + if (client_handles.clients == NULL) { // TODO: Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for client pointers."); } // Then fill the ServiceHandles with ready clients size_t client_handle_index = 0; - for (auto &client : clients) - { + for (auto & client : clients) { client_handles.clients[client_handle_index] = \ client->client_handle_->data; client_handle_index += 1; @@ -341,37 +314,34 @@ class Executor // TODO(wjwwood): Avoid redundant malloc's guard_condition_handles.guard_conditions = static_cast( std::malloc(sizeof(void *) * number_of_guard_conds)); - if (guard_condition_handles.guard_conditions == NULL) - { + if (guard_condition_handles.guard_conditions == NULL) { // TODO(wjwwood): Use a different error here? maybe std::bad_alloc? - throw std::runtime_error( - "Could not malloc for guard condition pointers."); + throw std::runtime_error("Could not malloc for guard condition pointers."); } // Put the global ctrl-c guard condition in assert(guard_condition_handles.guard_condition_count > 1); guard_condition_handles.guard_conditions[0] = \ - rclcpp::utilities::get_global_sigint_guard_condition()->data; + rclcpp::utilities::get_global_sigint_guard_condition()->data; // Put the executor's guard condition in guard_condition_handles.guard_conditions[1] = \ - interrupt_guard_condition_->data; + interrupt_guard_condition_->data; // Then fill the SubscriberHandles with ready subscriptions size_t guard_cond_handle_index = start_of_timer_guard_conds; - for (auto &timer : timers) - { + for (auto & timer : timers) { guard_condition_handles.guard_conditions[guard_cond_handle_index] = \ timer->guard_condition_->data; guard_cond_handle_index += 1; } // Now wait on the waitable subscriptions and timers - rmw_wait(&subscriber_handles, - &guard_condition_handles, - &service_handles, - &client_handles, - nonblocking); + rmw_wait( + &subscriber_handles, + &guard_condition_handles, + &service_handles, + &client_handles, + nonblocking); // If ctrl-c guard condition, return directly - if (guard_condition_handles.guard_conditions[0] != 0) - { + if (guard_condition_handles.guard_conditions[0] != 0) { // Make sure to free memory // TODO(wjwwood): Remove theses when the "Avoid redundant malloc's" // todo has been addressed. @@ -382,38 +352,30 @@ class Executor } // Add the new work to the class's list of things waiting to be executed // Starting with the subscribers - for (size_t i = 0; i < number_of_subscriptions; ++i) - { - void *handle = subscriber_handles.subscribers[i]; - if (handle) - { + for (size_t i = 0; i < number_of_subscriptions; ++i) { + void * handle = subscriber_handles.subscribers[i]; + if (handle) { subscriber_handles_.push_back(handle); } } // Then the timers, start with start_of_timer_guard_conds - for (size_t i = start_of_timer_guard_conds; i < number_of_guard_conds; ++i) - { - void *handle = guard_condition_handles.guard_conditions[i]; - if (handle) - { + for (size_t i = start_of_timer_guard_conds; i < number_of_guard_conds; ++i) { + void * handle = guard_condition_handles.guard_conditions[i]; + if (handle) { guard_condition_handles_.push_back(handle); } } // Then the services - for (size_t i = 0; i < number_of_services; ++i) - { - void *handle = service_handles.services[i]; - if (handle) - { + for (size_t i = 0; i < number_of_services; ++i) { + void * handle = service_handles.services[i]; + if (handle) { service_handles_.push_back(handle); } } // Then the clients - for (size_t i = 0; i < number_of_clients; ++i) - { - void *handle = client_handles.clients[i]; - if (handle) - { + for (size_t i = 0; i < number_of_clients; ++i) { + void * handle = client_handles.clients[i]; + if (handle) { client_handles_.push_back(handle); } } @@ -431,24 +393,18 @@ class Executor rclcpp::subscription::SubscriptionBase::SharedPtr get_subscription_by_handle(void * subscriber_handle) { - for (auto weak_node : weak_nodes_) - { + for (auto weak_node : weak_nodes_) { auto node = weak_node.lock(); - if (!node) - { + if (!node) { continue; } - for (auto weak_group : node->callback_groups_) - { + for (auto weak_group : node->callback_groups_) { auto group = weak_group.lock(); - if (!group) - { + if (!group) { continue; } - for (auto subscription : group->subscription_ptrs_) - { - if (subscription->subscription_handle_->data == subscriber_handle) - { + for (auto subscription : group->subscription_ptrs_) { + if (subscription->subscription_handle_->data == subscriber_handle) { return subscription; } } @@ -460,24 +416,18 @@ class Executor rclcpp::timer::TimerBase::SharedPtr get_timer_by_handle(void * guard_condition_handle) { - for (auto weak_node : weak_nodes_) - { + for (auto weak_node : weak_nodes_) { auto node = weak_node.lock(); - if (!node) - { + if (!node) { continue; } - for (auto weak_group : node->callback_groups_) - { + for (auto weak_group : node->callback_groups_) { auto group = weak_group.lock(); - if (!group) - { + if (!group) { continue; } - for (auto timer : group->timer_ptrs_) - { - if (timer->guard_condition_->data == guard_condition_handle) - { + for (auto timer : group->timer_ptrs_) { + if (timer->guard_condition_->data == guard_condition_handle) { return timer; } } @@ -489,24 +439,18 @@ class Executor rclcpp::service::ServiceBase::SharedPtr get_service_by_handle(void * service_handle) { - for (auto weak_node : weak_nodes_) - { + for (auto weak_node : weak_nodes_) { auto node = weak_node.lock(); - if (!node) - { + if (!node) { continue; } - for (auto weak_group : node->callback_groups_) - { + for (auto weak_group : node->callback_groups_) { auto group = weak_group.lock(); - if (!group) - { + if (!group) { continue; } - for (auto service : group->service_ptrs_) - { - if (service->service_handle_->data == service_handle) - { + for (auto service : group->service_ptrs_) { + if (service->service_handle_->data == service_handle) { return service; } } @@ -518,24 +462,18 @@ class Executor rclcpp::client::ClientBase::SharedPtr get_client_by_handle(void * client_handle) { - for (auto weak_node : weak_nodes_) - { + for (auto weak_node : weak_nodes_) { auto node = weak_node.lock(); - if (!node) - { + if (!node) { continue; } - for (auto weak_group : node->callback_groups_) - { + for (auto weak_group : node->callback_groups_) { auto group = weak_group.lock(); - if (!group) - { + if (!group) { continue; } - for (auto client : group->client_ptrs_) - { - if (client->client_handle_->data == client_handle) - { + for (auto client : group->client_ptrs_) { + if (client->client_handle_->data == client_handle) { return client; } } @@ -546,52 +484,46 @@ class Executor void - remove_subscriber_handle_from_subscriber_handles(void *handle) + remove_subscriber_handle_from_subscriber_handles(void * handle) { subscriber_handles_.remove(handle); } 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); } void - remove_service_handle_from_service_handles(void *handle) + remove_service_handle_from_service_handles(void * handle) { service_handles_.remove(handle); } void - remove_client_handle_from_client_handles(void *handle) + remove_client_handle_from_client_handles(void * handle) { client_handles_.remove(handle); } 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(); } - for (auto &weak_node : weak_nodes_) - { + for (auto & weak_node : weak_nodes_) { auto node = weak_node.lock(); - if (!node) - { + if (!node) { continue; } - for (auto &weak_group : node->callback_groups_) - { + for (auto & weak_group : node->callback_groups_) { auto callback_group = weak_group.lock(); - if (!callback_group) - { + if (!callback_group) { continue; } - if (callback_group == group) - { + if (callback_group == group) { return node; } } @@ -601,22 +533,17 @@ class Executor rclcpp::callback_group::CallbackGroup::SharedPtr 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(); - if (!node) - { + if (!node) { continue; } - for (auto &weak_group : node->callback_groups_) - { + for (auto & weak_group : node->callback_groups_) { auto group = weak_group.lock(); - for (auto &t : group->timer_ptrs_) - { - if (t == timer) - { + for (auto & t : group->timer_ptrs_) { + if (t == timer) { return group; } } @@ -626,24 +553,20 @@ class Executor } void - get_next_timer(std::shared_ptr &any_exec) + get_next_timer(std::shared_ptr & any_exec) { - for (auto handle : guard_condition_handles_) - { + for (auto handle : guard_condition_handles_) { auto timer = get_timer_by_handle(handle); - if (timer) - { + if (timer) { // Find the group for this handle and see if it can be serviced auto group = get_group_by_timer(timer); - if (!group) - { + if (!group) { // Group was not found, meaning the timer is not valid... // Remove it from the ready list and continue looking remove_guard_condition_handle_from_guard_condition_handles(handle); continue; } - if (!group->can_be_taken_from_.load()) - { + if (!group->can_be_taken_from_.load()) { // Group is mutually exclusive and is being used, so skip it for now // Leave it to be checked next time, but continue searching continue; @@ -662,22 +585,17 @@ class Executor rclcpp::callback_group::CallbackGroup::SharedPtr 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(); - if (!node) - { + if (!node) { continue; } - for (auto &weak_group : node->callback_groups_) - { + for (auto & weak_group : node->callback_groups_) { auto group = weak_group.lock(); - for (auto &sub : group->subscription_ptrs_) - { - if (sub == subscription) - { + for (auto & sub : group->subscription_ptrs_) { + if (sub == subscription) { return group; } } @@ -687,24 +605,20 @@ class Executor } void - get_next_subscription(std::shared_ptr &any_exec) + get_next_subscription(std::shared_ptr & any_exec) { - for (auto handle : subscriber_handles_) - { + for (auto handle : subscriber_handles_) { auto subscription = get_subscription_by_handle(handle); - if (subscription) - { + if (subscription) { // Find the group for this handle and see if it can be serviced auto group = get_group_by_subscription(subscription); - if (!group) - { + if (!group) { // Group was not found, meaning the subscription is not valid... // Remove it from the ready list and continue looking remove_subscriber_handle_from_subscriber_handles(handle); continue; } - if (!group->can_be_taken_from_.load()) - { + if (!group->can_be_taken_from_.load()) { // Group is mutually exclusive and is being used, so skip it for now // Leave it to be checked next time, but continue searching continue; @@ -723,22 +637,17 @@ class Executor rclcpp::callback_group::CallbackGroup::SharedPtr 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(); - if (!node) - { + if (!node) { continue; } - for (auto &weak_group : node->callback_groups_) - { + for (auto & weak_group : node->callback_groups_) { auto group = weak_group.lock(); - for (auto &serv : group->service_ptrs_) - { - if (serv == service) - { + for (auto & serv : group->service_ptrs_) { + if (serv == service) { return group; } } @@ -748,24 +657,20 @@ class Executor } void - get_next_service(std::shared_ptr &any_exec) + get_next_service(std::shared_ptr & any_exec) { - for (auto handle : service_handles_) - { + for (auto handle : service_handles_) { auto service = get_service_by_handle(handle); - if (service) - { + if (service) { // Find the group for this handle and see if it can be serviced auto group = get_group_by_service(service); - if (!group) - { + if (!group) { // Group was not found, meaning the service is not valid... // Remove it from the ready list and continue looking remove_service_handle_from_service_handles(handle); continue; } - if (!group->can_be_taken_from_.load()) - { + if (!group->can_be_taken_from_.load()) { // Group is mutually exclusive and is being used, so skip it for now // Leave it to be checked next time, but continue searching continue; @@ -784,22 +689,17 @@ class Executor rclcpp::callback_group::CallbackGroup::SharedPtr 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(); - if (!node) - { + if (!node) { continue; } - for (auto &weak_group : node->callback_groups_) - { + for (auto & weak_group : node->callback_groups_) { auto group = weak_group.lock(); - for (auto &cli : group->client_ptrs_) - { - if (cli == client) - { + for (auto & cli : group->client_ptrs_) { + if (cli == client) { return group; } } @@ -809,24 +709,20 @@ class Executor } void - get_next_client(std::shared_ptr &any_exec) + get_next_client(std::shared_ptr & any_exec) { - for (auto handle : client_handles_) - { + for (auto handle : client_handles_) { auto client = get_client_by_handle(handle); - if (client) - { + if (client) { // Find the group for this handle and see if it can be serviced auto group = get_group_by_client(client); - if (!group) - { + if (!group) { // Group was not found, meaning the service is not valid... // Remove it from the ready list and continue looking remove_client_handle_from_client_handles(handle); continue; } - if (!group->can_be_taken_from_.load()) - { + if (!group->can_be_taken_from_.load()) { // Group is mutually exclusive and is being used, so skip it for now // Leave it to be checked next time, but continue searching continue; @@ -849,26 +745,22 @@ class Executor std::shared_ptr any_exec(new AnyExecutable()); // Check the timers to see if there are any that are ready, if so return get_next_timer(any_exec); - if (any_exec->timer) - { + if (any_exec->timer) { return any_exec; } // Check the subscriptions to see if there are any that are ready get_next_subscription(any_exec); - if (any_exec->subscription) - { + if (any_exec->subscription) { return any_exec; } // Check the services to see if there are any that are ready get_next_service(any_exec); - if (any_exec->service) - { + if (any_exec->service) { return any_exec; } // Check the clients to see if there are any that are ready get_next_client(any_exec); - if (any_exec->client) - { + if (any_exec->client) { return any_exec; } // If there is neither a ready timer nor subscription, return a null ptr @@ -877,14 +769,13 @@ class Executor } std::shared_ptr - get_next_executable(bool nonblocking=false) + get_next_executable(bool nonblocking = false) { // Check to see if there are any subscriptions or timers needing service // TODO(wjwwood): improve run to run efficiency of this function auto any_exec = get_next_ready_executable(); // If there are none - if (!any_exec) - { + if (!any_exec) { // Wait for subscriptions or timers to work on wait_for_work(nonblocking); // Try again @@ -892,15 +783,14 @@ class Executor } // At this point any_exec should be valid with either a valid subscription // or a valid timer, or it should be a null shared_ptr - if (any_exec) - { + if (any_exec) { // If it is valid, check to see if the group is mutually exclusive or // not, then mark it accordingly if (any_exec->callback_group->type_ == \ - callback_group::CallbackGroupType::MutuallyExclusive) + callback_group::CallbackGroupType::MutuallyExclusive) { // It should not have been taken otherwise - assert(any_exec->callback_group->can_be_taken_from_.load() == true); + assert(any_exec->callback_group->can_be_taken_from_.load()); // Set to false to indicate something is being run from this group any_exec->callback_group->can_be_taken_from_.store(false); } @@ -914,13 +804,13 @@ class Executor RCLCPP_DISABLE_COPY(Executor); std::vector> weak_nodes_; - typedef std::list SubscriberHandles; + typedef std::list SubscriberHandles; SubscriberHandles subscriber_handles_; - typedef std::list GuardConditionHandles; + typedef std::list GuardConditionHandles; GuardConditionHandles guard_condition_handles_; - typedef std::list ServiceHandles; + typedef std::list ServiceHandles; ServiceHandles service_handles_; - typedef std::list ClientHandles; + typedef std::list ClientHandles; ClientHandles client_handles_; }; diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 18f5881316..8428aa65d0 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -26,7 +26,7 @@ namespace executors using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor; using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; -} -} +} // namespace executors +} // namespace rclcpp #endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */ diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 9248b44974..d16135c675 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -43,8 +43,7 @@ class MultiThreadedExecutor : public executor::Executor MultiThreadedExecutor() { number_of_threads_ = std::thread::hardware_concurrency(); - if (number_of_threads_ == 0) - { + if (number_of_threads_ == 0) { number_of_threads_ = 1; } } @@ -58,15 +57,13 @@ class MultiThreadedExecutor : public executor::Executor { std::lock_guard wait_lock(wait_mutex_); size_t thread_id_ = 1; // Use a _ suffix to avoid shadowing `rclcpp::thread_id` - for (size_t i = number_of_threads_; i > 0; --i) - { + for (size_t i = number_of_threads_; i > 0; --i) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id_++); threads.emplace_back(func); } } - for (auto &thread : threads) - { + for (auto & thread : threads) { thread.join(); } } @@ -81,13 +78,11 @@ class MultiThreadedExecutor : public executor::Executor void run(size_t this_thread_id) { rclcpp::thread_id = this_thread_id; - while (rclcpp::utilities::ok()) - { + while (rclcpp::utilities::ok()) { std::shared_ptr any_exec; { std::lock_guard wait_lock(wait_mutex_); - if (!rclcpp::utilities::ok()) - { + if (!rclcpp::utilities::ok()) { return; } any_exec = get_next_executable(); diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp index 1d67152986..ebcd51dd8d 100644 --- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -46,8 +46,7 @@ class SingleThreadedExecutor : public executor::Executor void spin() { - while (rclcpp::utilities::ok()) - { + while (rclcpp::utilities::ok()) { auto any_exec = get_next_executable(); execute_any_executable(any_exec); } diff --git a/rclcpp/include/rclcpp/macros.hpp b/rclcpp/include/rclcpp/macros.hpp index 1b67f20eee..d06b8d52b8 100644 --- a/rclcpp/include/rclcpp/macros.hpp +++ b/rclcpp/include/rclcpp/macros.hpp @@ -20,8 +20,8 @@ * Use in the private section of the class. */ #define RCLCPP_DISABLE_COPY(Class) \ - Class(const Class&) = delete; \ - Class& operator=(const Class&) = delete; + Class(const Class &) = delete; \ + Class & operator=(const Class &) = delete; /* Defines a make_shared static function on the class using perfect forwarding. * @@ -31,11 +31,11 @@ #define RCLCPP_MAKE_SHARED_DEFINITIONS(Class) \ typedef std::shared_ptr SharedPtr; \ \ - template \ + template \ static std::shared_ptr \ - make_shared(Args &&...args) \ + make_shared(Args && ... args) \ { \ - return std::make_shared(std::forward(args)...); \ + return std::make_shared(std::forward(args) ...); \ } #define RCLCPP_INFO(Args) std::cout << Args << std::endl; diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 45e5b760a7..3dea295d74 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -29,13 +29,19 @@ #include // Forward declaration of ROS middleware class -namespace rmw {struct rmw_node_t;} +namespace rmw +{ +struct rmw_node_t; +} // namespace rmw namespace rclcpp { // Forward declaration for friend statement -namespace executor {class Executor;} +namespace executor +{ +class Executor; +} // namespace executor namespace node { @@ -47,6 +53,7 @@ namespace node class Node { friend class rclcpp::executor::Executor; + public: RCLCPP_MAKE_SHARED_DEFINITIONS(Node); @@ -64,7 +71,8 @@ class Node create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); /* Create and return a Publisher. */ - template rclcpp::publisher::Publisher::SharedPtr + template + rclcpp::publisher::Publisher::SharedPtr create_publisher(std::string topic_name, size_t queue_size); /* Create and return a Subscription. */ @@ -74,46 +82,47 @@ class Node std::string topic_name, size_t queue_size, std::function &)> callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create a timer. */ rclcpp::timer::WallTimer::SharedPtr create_wall_timer( std::chrono::nanoseconds period, rclcpp::timer::CallbackType callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); rclcpp::timer::WallTimer::SharedPtr create_wall_timer( std::chrono::duration period, 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 std::weak_ptr CallbackGroupWeakPtr; typedef std::list CallbackGroupWeakPtrList; /* Create and return a Client. */ - template + template typename rclcpp::client::Client::SharedPtr create_client( std::string service_name, - rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Service. */ - template + template typename rclcpp::service::Service::SharedPtr create_service( std::string service_name, - std::function &, - std::shared_ptr&)> callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); + std::function &, + std::shared_ptr &)> callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); private: RCLCPP_DISABLE_COPY(Node); bool - group_in_node(callback_group::CallbackGroup::SharedPtr &group); + group_in_node(callback_group::CallbackGroup::SharedPtr & group); std::string name_; @@ -135,11 +144,13 @@ class Node } /* namespace rclcpp */ #define RCLCPP_REGISTER_NODE(Class) RMW_EXPORT \ -rclcpp::node::Node::SharedPtr \ -create_node() \ -{ \ - return rclcpp::node::Node::SharedPtr(new Class(rclcpp::contexts::default_context::DefaultContext::make_shared())); \ -} + rclcpp::node::Node::SharedPtr \ + create_node() \ + { \ + return rclcpp::node::Node::SharedPtr(new Class( \ + rclcpp::contexts::default_context::DefaultContext:: \ + make_shared())); \ + } #ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_ // Template implementations diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index e4bf487792..98d404ba26 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -34,12 +34,12 @@ using namespace rclcpp::node; using rclcpp::contexts::default_context::DefaultContext; 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) - : name_(node_name), context_(context), - number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0) +: name_(node_name), context_(context), + number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0) { node_handle_ = rmw_create_node(name_.c_str()); using rclcpp::callback_group::CallbackGroupType; @@ -71,14 +71,12 @@ Node::create_publisher(std::string topic_name, size_t queue_size) } 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; - for (auto &weak_group : this->callback_groups_) - { + for (auto & weak_group : this->callback_groups_) { auto cur_group = weak_group.lock(); - if (cur_group && cur_group == group) - { + if (cur_group && (cur_group == group)) { group_belongs_to_this_node = true; } } @@ -100,21 +98,18 @@ Node::create_subscription( using namespace rclcpp::subscription; - auto sub = Subscription::make_shared(subscriber_handle, - topic_name, - callback); + auto sub = Subscription::make_shared( + subscriber_handle, + topic_name, + callback); auto sub_base_ptr = std::dynamic_pointer_cast(sub); - if (group) - { - if (!group_in_node(group)) - { + if (group) { + if (!group_in_node(group)) { // TODO: use custom exception throw std::runtime_error("Cannot create timer, group not in node."); } group->add_subscription(sub_base_ptr); - } - else - { + } else { default_callback_group_->add_subscription(sub_base_ptr); } number_of_subscriptions_++; @@ -122,22 +117,19 @@ Node::create_subscription( } rclcpp::timer::WallTimer::SharedPtr -Node::create_wall_timer(std::chrono::nanoseconds period, - rclcpp::timer::CallbackType callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group) +Node::create_wall_timer( + std::chrono::nanoseconds period, + rclcpp::timer::CallbackType callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group) { auto timer = rclcpp::timer::WallTimer::make_shared(period, callback); - if (group) - { - if (!group_in_node(group)) - { + if (group) { + if (!group_in_node(group)) { // TODO: use custom exception throw std::runtime_error("Cannot create timer, group not in node."); } group->add_timer(timer); - } - else - { + } else { default_callback_group_->add_timer(timer); } number_of_timers_++; @@ -171,21 +163,18 @@ Node::create_client( using namespace rclcpp::client; - auto cli = Client::make_shared(client_handle, - service_name); + auto cli = Client::make_shared( + client_handle, + service_name); auto cli_base_ptr = std::dynamic_pointer_cast(cli); - if (group) - { - if (!group_in_node(group)) - { + if (group) { + if (!group_in_node(group)) { // TODO(esteve): use custom exception throw std::runtime_error("Cannot create client, group not in node."); } group->add_client(cli_base_ptr); - } - else - { + } else { default_callback_group_->add_client(cli_base_ptr); } number_of_clients_++; @@ -193,12 +182,12 @@ Node::create_client( return cli; } -template +template typename service::Service::SharedPtr Node::create_service( std::string service_name, std::function &, - std::shared_ptr&)> callback, + std::shared_ptr &)> callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { using rosidl_generator_cpp::get_service_type_support_handle; @@ -210,22 +199,19 @@ Node::create_service( using namespace rclcpp::service; - auto serv = Service::make_shared(service_handle, - service_name, - callback); + auto serv = Service::make_shared( + service_handle, + service_name, + callback); auto serv_base_ptr = std::dynamic_pointer_cast(serv); - if (group) - { - if (!group_in_node(group)) - { + if (group) { + if (!group_in_node(group)) { // TODO: use custom exception throw std::runtime_error("Cannot create service, group not in node."); } group->add_service(serv_base_ptr); - } - else - { + } else { default_callback_group_->add_service(serv_base_ptr); } number_of_services_++; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index e401b0420e..7ae9450065 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -25,7 +25,10 @@ namespace rclcpp { // Forward declaration for friend statement -namespace node {class Node;} +namespace node +{ +class Node; +} // namespace node namespace publisher { @@ -36,11 +39,12 @@ class Publisher RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher); Publisher(rmw_publisher_t * publisher_handle) - : publisher_handle_(publisher_handle) + : publisher_handle_(publisher_handle) {} - template void - publish(std::shared_ptr &msg) + template + void + publish(std::shared_ptr & msg) { rmw_publish(publisher_handle_, msg.get()); } diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 602638324e..6b74681e09 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -48,11 +48,11 @@ class GenericRate : public RateBase RCLCPP_MAKE_SHARED_DEFINITIONS(GenericRate); GenericRate(double rate) - : GenericRate( - duration_cast(duration(1.0/rate))) + : GenericRate( + duration_cast(duration(1.0 / rate))) {} GenericRate(std::chrono::nanoseconds period) - : period_(period), last_interval_(Clock::now()) + : period_(period), last_interval_(Clock::now()) {} virtual bool @@ -63,8 +63,7 @@ class GenericRate : public RateBase // Time of next interval auto next_interval = last_interval_ + period_; // Detect backwards time flow - if (now < last_interval_) - { + if (now < last_interval_) { // Best thing to do is to set the next_interval to now + period next_interval = now + period_; } @@ -73,13 +72,11 @@ class GenericRate : public RateBase // Update the interval last_interval_ += period_; // If the time_to_sleep is negative or zero, don't sleep - if (time_to_sleep <= std::chrono::seconds(0)) - { + if (time_to_sleep <= std::chrono::seconds(0)) { // If an entire cycle was missed then reset next interval. // This might happen if the loop took more than a cycle. // Or if time jumps forward. - if (now > next_interval + period_) - { + if (now > next_interval + period_) { last_interval_ = now + period_; } // Either way do not sleep and return false @@ -113,7 +110,7 @@ class GenericRate : public RateBase typedef GenericRate Rate; typedef GenericRate WallRate; -} -} +} // namespace rate +} // namespace rclcpp #endif /* RCLCPP_RCLCPP_RATE_HPP_ */ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 797cc510fa..040cf3897e 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -26,26 +26,26 @@ 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::duration(s)); + return std::chrono::duration_cast( + std::chrono::duration(s)); } 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 -operator "" _ns(long double ns) +operator"" _ns(long double ns) { - return std::chrono::duration_cast( - std::chrono::duration(ns)); + return std::chrono::duration_cast( + std::chrono::duration(ns)); } using rclcpp::node::Node; @@ -63,13 +63,13 @@ using rclcpp::utilities::shutdown; using rclcpp::utilities::init; using rclcpp::utilities::sleep_for; -void spin_some(Node::SharedPtr &node_ptr) +void spin_some(Node::SharedPtr & node_ptr) { rclcpp::executors::SingleThreadedExecutor executor; executor.spin_node_some(node_ptr); } -void spin(Node::SharedPtr &node_ptr) +void spin(Node::SharedPtr & node_ptr) { rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node_ptr); diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 2cf0519895..ad1376c64c 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -28,7 +28,10 @@ namespace rclcpp { // Forward declaration for friend statement -namespace executor {class Executor;} +namespace executor +{ +class Executor; +} // namespace executor namespace service { @@ -36,18 +39,19 @@ namespace service class ServiceBase { friend class rclcpp::executor::Executor; + public: RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase); - ServiceBase(rmw_service_t * service_handle, - const std::string service_name) + ServiceBase( + rmw_service_t * service_handle, + const std::string service_name) : service_handle_(service_handle), service_name_(service_name) {} ~ServiceBase() { - if (service_handle_ != nullptr) - { + if (service_handle_ != nullptr) { rmw_destroy_service(service_handle_); service_handle_ = nullptr; } @@ -65,8 +69,9 @@ class ServiceBase virtual std::shared_ptr create_request() = 0; virtual std::shared_ptr create_request_header() = 0; - virtual void handle_request(std::shared_ptr &request_header, - std::shared_ptr &request) = 0; + virtual void handle_request( + std::shared_ptr & request_header, + std::shared_ptr & request) = 0; private: RCLCPP_DISABLE_COPY(ServiceBase); @@ -81,13 +86,14 @@ class Service : public ServiceBase { public: typedef std::function< - void(const std::shared_ptr &, - std::shared_ptr&)> CallbackType; + void (const std::shared_ptr &, + std::shared_ptr &)> CallbackType; RCLCPP_MAKE_SHARED_DEFINITIONS(Service); - Service(rmw_service_t * service_handle, - const std::string &service_name, - CallbackType callback) + Service( + rmw_service_t * service_handle, + const std::string & service_name, + CallbackType callback) : ServiceBase(service_handle, service_name), callback_(callback) {} @@ -103,7 +109,7 @@ class Service : public ServiceBase return std::shared_ptr(new rmw_request_id_t); } - void handle_request(std::shared_ptr &request_header, std::shared_ptr &request) + void handle_request(std::shared_ptr & request_header, std::shared_ptr & request) { auto typed_request = std::static_pointer_cast(request); auto typed_request_header = std::static_pointer_cast(request_header); @@ -112,8 +118,9 @@ class Service : public ServiceBase send_response(typed_request_header, response); } - void send_response(std::shared_ptr &req_id, - std::shared_ptr &response) + void send_response( + std::shared_ptr & req_id, + std::shared_ptr & response) { rmw_send_response(get_service_handle(), req_id.get(), response.get()); } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index c4c22c6fd0..8c13e7a2de 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -27,7 +27,10 @@ namespace rclcpp { // Forward declaration is for friend statement in SubscriptionBase -namespace executor {class Executor;} +namespace executor +{ +class Executor; +} // namespace executor namespace subscription { @@ -35,13 +38,14 @@ namespace subscription class SubscriptionBase { friend class rclcpp::executor::Executor; + public: RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase); SubscriptionBase( rmw_subscription_t * subscription_handle, - std::string &topic_name) - : subscription_handle_(subscription_handle), topic_name_(topic_name) + std::string & topic_name) + : subscription_handle_(subscription_handle), topic_name_(topic_name) {} std::string get_topic_name() @@ -50,7 +54,7 @@ class SubscriptionBase } virtual std::shared_ptr create_message() = 0; - virtual void handle_message(std::shared_ptr &message) = 0; + virtual void handle_message(std::shared_ptr & message) = 0; private: RCLCPP_DISABLE_COPY(SubscriptionBase); @@ -64,13 +68,14 @@ template class Subscription : public SubscriptionBase { public: - typedef std::function &)> CallbackType; + typedef std::function &)> CallbackType; RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription); - Subscription(rmw_subscription_t * subscription_handle, - std::string &topic_name, - CallbackType callback) - : SubscriptionBase(subscription_handle, topic_name), callback_(callback) + Subscription( + rmw_subscription_t * subscription_handle, + std::string & topic_name, + CallbackType callback) + : SubscriptionBase(subscription_handle, topic_name), callback_(callback) {} std::shared_ptr create_message() @@ -78,7 +83,7 @@ class Subscription : public SubscriptionBase return std::shared_ptr(new MessageT()); } - void handle_message(std::shared_ptr &message) + void handle_message(std::shared_ptr & message) { auto typed_message = std::static_pointer_cast(message); callback_(typed_message); diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 335659978b..d378669278 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -30,23 +30,27 @@ namespace rclcpp { // Forward declaration is for friend statement in GenericTimer -namespace executor {class Executor;} +namespace executor +{ +class Executor; +} // namespace executor namespace timer { -typedef std::function CallbackType; +typedef std::function CallbackType; class TimerBase { friend class rclcpp::executor::Executor; + public: RCLCPP_MAKE_SHARED_DEFINITIONS(TimerBase); TimerBase(std::chrono::nanoseconds period, CallbackType callback) - : period_(period), - callback_(callback), - canceled_(false) + : period_(period), + callback_(callback), + canceled_(false) { guard_condition_ = rmw_create_guard_condition(); } @@ -72,11 +76,12 @@ template class GenericTimer : public TimerBase { friend class rclcpp::executor::Executor; + public: RCLCPP_MAKE_SHARED_DEFINITIONS(GenericTimer); GenericTimer(std::chrono::nanoseconds period, CallbackType callback) - : TimerBase(period, callback), loop_rate_(period) + : TimerBase(period, callback), loop_rate_(period) { thread_ = std::thread(&GenericTimer::run, this); } @@ -89,11 +94,9 @@ class GenericTimer : public TimerBase void run() { - while (rclcpp::utilities::ok() && !this->canceled_) - { + while (rclcpp::utilities::ok() && !this->canceled_) { loop_rate_.sleep(); - if (!rclcpp::utilities::ok()) - { + if (!rclcpp::utilities::ok()) { return; } rmw_trigger_guard_condition(guard_condition_); diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 959810a818..5968683474 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -36,55 +36,53 @@ namespace { - volatile sig_atomic_t g_signal_status = 0; - rmw_guard_condition_t * g_sigint_guard_cond_handle = \ - rmw_create_guard_condition(); - std::condition_variable g_interrupt_condition_variable; - std::mutex g_interrupt_mutex; +volatile sig_atomic_t g_signal_status = 0; +rmw_guard_condition_t * g_sigint_guard_cond_handle = \ + rmw_create_guard_condition(); +std::condition_variable g_interrupt_condition_variable; +std::mutex g_interrupt_mutex; #ifdef HAS_SIGACTION - struct sigaction old_action; +struct sigaction old_action; #else - void (*old_signal_handler)(int) = 0; +void (* old_signal_handler)(int) = 0; #endif - void +void #ifdef HAS_SIGACTION - signal_handler(int signal_value, siginfo_t *siginfo, void *context) +signal_handler(int signal_value, siginfo_t * siginfo, void * context) #else - signal_handler(int signal_value) +signal_handler(int signal_value) #endif - { - // TODO(wjwwood): remove - std::cout << "signal_handler(" << signal_value << ")" << std::endl; +{ + // TODO(wjwwood): remove + std::cout << "signal_handler(" << signal_value << ")" << std::endl; #ifdef HAS_SIGACTION - if (old_action.sa_flags & SA_SIGINFO) - { - if (old_action.sa_sigaction != NULL) - { - old_action.sa_sigaction(signal_value, siginfo, context); - } + if (old_action.sa_flags & SA_SIGINFO) { + if (old_action.sa_sigaction != NULL) { + old_action.sa_sigaction(signal_value, siginfo, context); } - else + } else { + // *INDENT-OFF* + if ( + old_action.sa_handler != NULL && // Is set + old_action.sa_handler != SIG_DFL && // Is not default + old_action.sa_handler != SIG_IGN) // Is not ignored + // *INDENT-ON* { - if (old_action.sa_handler != NULL && // Is set - old_action.sa_handler != SIG_DFL && // Is not default - old_action.sa_handler != SIG_IGN) // Is not ignored - { - old_action.sa_handler(signal_value); - } + old_action.sa_handler(signal_value); } + } #else - if (old_signal_handler) - { - old_signal_handler(signal_value); - } -#endif - g_signal_status = signal_value; - rmw_trigger_guard_condition(g_sigint_guard_cond_handle); - g_interrupt_condition_variable.notify_all(); + if (old_signal_handler) { + old_signal_handler(signal_value); } +#endif + g_signal_status = signal_value; + rmw_trigger_guard_condition(g_sigint_guard_cond_handle); + g_interrupt_condition_variable.notify_all(); } +} // namespace namespace rclcpp { @@ -95,7 +93,7 @@ namespace utilities { void -init(int argc, char *argv[]) +init(int argc, char * argv[]) { // TODO(wjwwood): Handle rmw_ret_t's value. rmw_init(); @@ -112,11 +110,12 @@ init(int argc, char *argv[]) if (::old_signal_handler == SIG_ERR) #endif { + // *INDENT-OFF* throw std::runtime_error( - std::string("Failed to set SIGINT signal handler: (" + - std::to_string(errno) + ")") + + std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") + // TODO(wjwwood): use strerror_r on POSIX and strerror_s on Windows. std::strerror(errno)); + // *INDENT-ON* } } @@ -142,7 +141,7 @@ get_global_sigint_guard_condition() } 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 std::unique_lock lock(::g_interrupt_mutex); diff --git a/rclcpp/src/node_main.cpp b/rclcpp/src/node_main.cpp index 3a896c59b1..5d4554fabb 100644 --- a/rclcpp/src/node_main.cpp +++ b/rclcpp/src/node_main.cpp @@ -17,7 +17,7 @@ // This forward declaration is implemented by the RCLCPP_REGISTER_NODE macro RMW_IMPORT rclcpp::Node::SharedPtr create_node(); -int main(int argc, char **argv) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor;