From f80b89a6877b515fd371f2d983200137c7428a22 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 7 Apr 2015 10:57:33 -0700 Subject: [PATCH 1/6] code style only --- rclcpp/include/rclcpp/callback_group.hpp | 24 +- rclcpp/include/rclcpp/client.hpp | 33 +- .../rclcpp/contexts/default_context.hpp | 6 +- rclcpp/include/rclcpp/executor.hpp | 461 +++++++----------- rclcpp/include/rclcpp/executors.hpp | 4 +- .../executors/multi_threaded_executor.hpp | 15 +- .../executors/single_threaded_executor.hpp | 3 +- rclcpp/include/rclcpp/macros.hpp | 10 +- rclcpp/include/rclcpp/node.hpp | 44 +- rclcpp/include/rclcpp/node_impl.hpp | 80 ++- rclcpp/include/rclcpp/publisher.hpp | 10 +- rclcpp/include/rclcpp/rate.hpp | 15 +- rclcpp/include/rclcpp/rclcpp.hpp | 24 +- rclcpp/include/rclcpp/service.hpp | 41 +- rclcpp/include/rclcpp/subscription.hpp | 21 +- rclcpp/include/rclcpp/timer.hpp | 19 +- rclcpp/include/rclcpp/utilities.hpp | 72 ++- rclcpp/src/node_main.cpp | 2 +- 18 files changed, 392 insertions(+), 492 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 455783b60e..75ec1f127b 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -28,18 +28,28 @@ namespace rclcpp { // Forward declarations for friend statement in class CallbackGroup -namespace node {class Node;} -namespace executor {class Executor;} +namespace node +{ +class Node; +} // namespace node +namespace executor +{ +class Executor; +} // namespace executor namespace callback_group { -enum class CallbackGroupType {MutuallyExclusive, Reentrant}; +enum class CallbackGroupType { + MutuallyExclusive, + Reentrant +}; class CallbackGroup { friend class rclcpp::node::Node; friend class rclcpp::executor::Executor; + public: RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup); @@ -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..2233f35263 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) - : client_handle_(client_handle), service_name_(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,13 +87,12 @@ 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) - : ClientBase(client_handle, service_name) + Client(rmw_client_t * client_handle, const std::string & service_name) + : ClientBase(client_handle, service_name) {} std::shared_ptr create_response() @@ -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; 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..f1c1e67709 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -42,8 +42,7 @@ class Executor 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,14 +51,12 @@ 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."); @@ -71,31 +68,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); @@ -116,26 +112,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 +138,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 +154,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 +182,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 +210,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 +251,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 +270,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 +289,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,8 +311,7 @@ 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."); @@ -350,28 +319,27 @@ class Executor // 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 +350,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 +391,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 +414,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 +437,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 +460,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 +482,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 +531,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 +551,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 +583,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 +603,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 +635,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 +655,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 +687,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 +707,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 +743,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 +767,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 +781,13 @@ 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); } @@ -913,14 +800,14 @@ class Executor private: RCLCPP_DISABLE_COPY(Executor); - std::vector> weak_nodes_; - typedef std::list SubscriberHandles; + std::vector > weak_nodes_; + 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..68a009b3a1 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,46 @@ 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::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 +143,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..4c0a79a910 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -39,7 +39,7 @@ Node::Node(std::string node_name) 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) + 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..82e512a880 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 { @@ -39,8 +42,9 @@ class Publisher : 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..3eb54e904e 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -49,7 +49,7 @@ class GenericRate : public RateBase GenericRate(double rate) : GenericRate( - duration_cast(duration(1.0/rate))) + duration_cast(duration(1.0 / rate))) {} GenericRate(std::chrono::nanoseconds period) : period_(period), last_interval_(Clock::now()) @@ -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..2a0cb800b5 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) - : service_handle_(service_handle), service_name_(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,14 +86,15 @@ 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) - : ServiceBase(service_handle, service_name), callback_(callback) + Service( + rmw_service_t * service_handle, + const std::string & service_name, + CallbackType callback) + : ServiceBase(service_handle, service_name), callback_(callback) {} std::shared_ptr create_request() @@ -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..0d646ecc3d 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,12 +38,13 @@ 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) + std::string & topic_name) : subscription_handle_(subscription_handle), topic_name_(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,12 +68,13 @@ 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) + Subscription( + rmw_subscription_t * subscription_handle, + std::string & topic_name, + CallbackType callback) : SubscriptionBase(subscription_handle, topic_name), callback_(callback) {} @@ -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..4a9ffdb45b 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) + callback_(callback), + canceled_(false) { guard_condition_ = rmw_create_guard_condition(); } @@ -72,6 +76,7 @@ template class GenericTimer : public TimerBase { friend class rclcpp::executor::Executor; + public: RCLCPP_MAKE_SHARED_DEFINITIONS(GenericTimer); @@ -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..d4dc2df4b9 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -36,55 +36,51 @@ 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 - { - 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); - } + } else { + if ( + old_action.sa_handler != NULL && // Is set + old_action.sa_handler != SIG_DFL && // Is not default + old_action.sa_handler != SIG_IGN // Is not ignored + ) { + old_action.sa_handler(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 +91,7 @@ namespace utilities { void -init(int argc, char *argv[]) +init(int argc, char * argv[]) { // TODO(wjwwood): Handle rmw_ret_t's value. rmw_init(); @@ -114,7 +110,7 @@ init(int argc, char *argv[]) { throw std::runtime_error( 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. std::strerror(errno)); } @@ -142,7 +138,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; From b114db22130ba3d4ef7239859fcf7cb206d33181 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 7 Apr 2015 11:48:55 -0700 Subject: [PATCH 2/6] update --- rclcpp/include/rclcpp/executor.hpp | 3 ++- rclcpp/include/rclcpp/utilities.hpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index f1c1e67709..7d633bd16e 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -785,7 +785,8 @@ class Executor // 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()); // Set to false to indicate something is being run from this group diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index d4dc2df4b9..830334080e 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -66,8 +66,8 @@ signal_handler(int signal_value) if ( old_action.sa_handler != NULL && // Is set old_action.sa_handler != SIG_DFL && // Is not default - old_action.sa_handler != SIG_IGN // Is not ignored - ) { + old_action.sa_handler != SIG_IGN) // Is not ignored + { old_action.sa_handler(signal_value); } } From 9e735e35a0142b0a7447f95196baf6d8b206badb Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 7 Apr 2015 11:55:55 -0700 Subject: [PATCH 3/6] update --- rclcpp/include/rclcpp/node.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 68a009b3a1..3dea295d74 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -113,8 +113,9 @@ class Node typename rclcpp::service::Service::SharedPtr create_service( std::string service_name, - std::function &, - std::shared_ptr &)> callback, + std::function &, + std::shared_ptr &)> callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); private: From 9a6b48e2fcafb1e99c60d5efe2033b66dbd2d44a Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 7 Apr 2015 12:07:57 -0700 Subject: [PATCH 4/6] update --- rclcpp/include/rclcpp/client.hpp | 2 +- rclcpp/include/rclcpp/executor.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 2233f35263..a8e37811b8 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -144,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/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 7d633bd16e..d533fead28 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -801,7 +801,7 @@ class Executor private: RCLCPP_DISABLE_COPY(Executor); - std::vector > weak_nodes_; + std::vector> weak_nodes_; typedef std::list SubscriberHandles; SubscriberHandles subscriber_handles_; typedef std::list GuardConditionHandles; From c6b8fb2de0da06b2dd273993ba8c67c2b42ee9df Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 7 Apr 2015 15:20:43 -0700 Subject: [PATCH 5/6] update --- rclcpp/include/rclcpp/callback_group.hpp | 2 +- rclcpp/include/rclcpp/client.hpp | 4 ++-- rclcpp/include/rclcpp/executor.hpp | 14 ++++++++------ rclcpp/include/rclcpp/node_impl.hpp | 4 ++-- rclcpp/include/rclcpp/publisher.hpp | 2 +- rclcpp/include/rclcpp/rate.hpp | 4 ++-- rclcpp/include/rclcpp/service.hpp | 4 ++-- rclcpp/include/rclcpp/subscription.hpp | 4 ++-- rclcpp/include/rclcpp/timer.hpp | 4 ++-- rclcpp/include/rclcpp/utilities.hpp | 3 +-- 10 files changed, 23 insertions(+), 22 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 75ec1f127b..e4eb0c914f 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -54,7 +54,7 @@ class CallbackGroup 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: diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index a8e37811b8..3e532f3982 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -45,7 +45,7 @@ class ClientBase RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase); ClientBase(rmw_client_t * client_handle, const std::string & service_name) - : client_handle_(client_handle), service_name_(service_name) + : client_handle_(client_handle), service_name_(service_name) {} ~ClientBase() @@ -92,7 +92,7 @@ class Client : public ClientBase RCLCPP_MAKE_SHARED_DEFINITIONS(Client); Client(rmw_client_t * client_handle, const std::string & service_name) - : ClientBase(client_handle, service_name) + : ClientBase(client_handle, service_name) {} std::shared_ptr create_response() diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index d533fead28..4a99f58978 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -38,7 +38,9 @@ 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() { @@ -58,8 +60,7 @@ class Executor auto node = weak_node.lock(); 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); @@ -100,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; @@ -313,8 +316,7 @@ class Executor std::malloc(sizeof(void *) * number_of_guard_conds)); 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); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 4c0a79a910..98d404ba26 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -34,11 +34,11 @@ 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), +: 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()); diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 82e512a880..7ae9450065 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -39,7 +39,7 @@ class Publisher RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher); Publisher(rmw_publisher_t * publisher_handle) - : publisher_handle_(publisher_handle) + : publisher_handle_(publisher_handle) {} template diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 3eb54e904e..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( + : 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 diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 2a0cb800b5..ad1376c64c 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -46,7 +46,7 @@ class ServiceBase ServiceBase( rmw_service_t * service_handle, const std::string service_name) - : service_handle_(service_handle), service_name_(service_name) + : service_handle_(service_handle), service_name_(service_name) {} ~ServiceBase() @@ -94,7 +94,7 @@ class Service : public ServiceBase rmw_service_t * service_handle, const std::string & service_name, CallbackType callback) - : ServiceBase(service_handle, service_name), callback_(callback) + : ServiceBase(service_handle, service_name), callback_(callback) {} std::shared_ptr create_request() diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 0d646ecc3d..8c13e7a2de 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -45,7 +45,7 @@ class SubscriptionBase SubscriptionBase( rmw_subscription_t * subscription_handle, std::string & topic_name) - : subscription_handle_(subscription_handle), topic_name_(topic_name) + : subscription_handle_(subscription_handle), topic_name_(topic_name) {} std::string get_topic_name() @@ -75,7 +75,7 @@ class Subscription : public SubscriptionBase rmw_subscription_t * subscription_handle, std::string & topic_name, CallbackType callback) - : SubscriptionBase(subscription_handle, topic_name), callback_(callback) + : SubscriptionBase(subscription_handle, topic_name), callback_(callback) {} std::shared_ptr create_message() diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 4a9ffdb45b..d378669278 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -48,7 +48,7 @@ class TimerBase RCLCPP_MAKE_SHARED_DEFINITIONS(TimerBase); TimerBase(std::chrono::nanoseconds period, CallbackType callback) - : period_(period), + : period_(period), callback_(callback), canceled_(false) { @@ -81,7 +81,7 @@ class GenericTimer : public TimerBase 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); } diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 830334080e..1d04451303 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -109,8 +109,7 @@ init(int argc, char * argv[]) #endif { 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)); } From 8054786c0945d5e3f2a9b7b58593406b0f836602 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 7 Apr 2015 15:30:49 -0700 Subject: [PATCH 6/6] explicitly ignore remaining uncrustify issues --- rclcpp/include/rclcpp/utilities.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 1d04451303..5968683474 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -63,10 +63,12 @@ signal_handler(int signal_value) old_action.sa_sigaction(signal_value, siginfo, context); } } 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* { old_action.sa_handler(signal_value); } @@ -108,10 +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) + ")") + // TODO(wjwwood): use strerror_r on POSIX and strerror_s on Windows. std::strerror(errno)); + // *INDENT-ON* } }