diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index d117376517..8aee47ac91 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -62,7 +62,6 @@ void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_al return std::allocator_traits::allocate(*typed_allocator, size); } - // Convert a std::allocator_traits-formatted Allocator into an rcl allocator template< typename T, diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp index 8e44ea897a..977a579ab3 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp @@ -30,13 +30,11 @@ class AllocatorDeleter using AllocRebind = typename std::allocator_traits::template rebind_alloc; public: - AllocatorDeleter() - : allocator_(nullptr) + AllocatorDeleter() : allocator_(nullptr) { } - explicit AllocatorDeleter(Allocator * a) - : allocator_(a) + explicit AllocatorDeleter(Allocator * a) : allocator_(a) { } @@ -71,16 +69,16 @@ class AllocatorDeleter template void set_allocator_for_deleter(D * deleter, Alloc * alloc) { - (void) alloc; - (void) deleter; + (void)alloc; + (void)deleter; throw std::runtime_error("Reached unexpected template specialization"); } template void set_allocator_for_deleter(std::default_delete * deleter, std::allocator * alloc) { - (void) deleter; - (void) alloc; + (void)deleter; + (void)alloc; } template @@ -94,11 +92,11 @@ void set_allocator_for_deleter(AllocatorDeleter * deleter, Alloc * alloc) template using Deleter = typename std::conditional< - std::is_same::template rebind_alloc, - typename std::allocator::template rebind::other>::value, + std::is_same< + typename std::allocator_traits::template rebind_alloc, + typename std::allocator::template rebind::other>::value, std::default_delete, - AllocatorDeleter - >::type; + AllocatorDeleter>::type; } // namespace allocator } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 24d1e9f6ce..2bb065a0fd 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -31,17 +31,13 @@ template class AnyServiceCallback { private: - using SharedPtrCallback = std::function< - void ( - const std::shared_ptr, - std::shared_ptr - )>; - using SharedPtrWithRequestHeaderCallback = std::function< - void ( - const std::shared_ptr, - const std::shared_ptr, - std::shared_ptr - )>; + using SharedPtrCallback = std::function, + std::shared_ptr)>; + using SharedPtrWithRequestHeaderCallback = std::function, + const std::shared_ptr, + std::shared_ptr)>; SharedPtrCallback shared_ptr_callback_; SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_; @@ -49,19 +45,16 @@ class AnyServiceCallback public: AnyServiceCallback() : shared_ptr_callback_(nullptr), shared_ptr_with_request_header_callback_(nullptr) - {} + { + } AnyServiceCallback(const AnyServiceCallback &) = default; template< typename CallbackT, typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrCallback - >::value - >::type * = nullptr - > + rclcpp::function_traits::same_arguments::value>::type * = + nullptr> void set(CallbackT callback) { shared_ptr_callback_ = callback; @@ -69,13 +62,9 @@ class AnyServiceCallback template< typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrWithRequestHeaderCallback - >::value - >::type * = nullptr - > + typename std::enable_if::value>::type * = nullptr> void set(CallbackT callback) { shared_ptr_with_request_header_callback_ = callback; diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 86369b126e..90cea90faa 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -39,15 +39,15 @@ class AnySubscriptionCallback using ConstMessageSharedPtr = std::shared_ptr; using MessageUniquePtr = std::unique_ptr; - using SharedPtrCallback = std::function)>; + using SharedPtrCallback = std::function)>; using SharedPtrWithInfoCallback = - std::function, const rmw_message_info_t &)>; - using ConstSharedPtrCallback = std::function)>; + std::function, const rmw_message_info_t &)>; + using ConstSharedPtrCallback = std::function)>; using ConstSharedPtrWithInfoCallback = - std::function, const rmw_message_info_t &)>; - using UniquePtrCallback = std::function; + std::function, const rmw_message_info_t &)>; + using UniquePtrCallback = std::function; using UniquePtrWithInfoCallback = - std::function; + std::function; SharedPtrCallback shared_ptr_callback_; SharedPtrWithInfoCallback shared_ptr_with_info_callback_; @@ -58,9 +58,12 @@ class AnySubscriptionCallback public: explicit AnySubscriptionCallback(std::shared_ptr allocator) - : shared_ptr_callback_(nullptr), shared_ptr_with_info_callback_(nullptr), - const_shared_ptr_callback_(nullptr), const_shared_ptr_with_info_callback_(nullptr), - unique_ptr_callback_(nullptr), unique_ptr_with_info_callback_(nullptr) + : shared_ptr_callback_(nullptr), + shared_ptr_with_info_callback_(nullptr), + const_shared_ptr_callback_(nullptr), + const_shared_ptr_with_info_callback_(nullptr), + unique_ptr_callback_(nullptr), + unique_ptr_with_info_callback_(nullptr) { message_allocator_ = std::make_shared(*allocator.get()); allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); @@ -71,12 +74,8 @@ class AnySubscriptionCallback template< typename CallbackT, typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrCallback - >::value - >::type * = nullptr - > + rclcpp::function_traits::same_arguments::value>::type * = + nullptr> void set(CallbackT callback) { shared_ptr_callback_ = callback; @@ -85,12 +84,8 @@ class AnySubscriptionCallback template< typename CallbackT, typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrWithInfoCallback - >::value - >::type * = nullptr - > + rclcpp::function_traits::same_arguments::value>:: + type * = nullptr> void set(CallbackT callback) { shared_ptr_with_info_callback_ = callback; @@ -99,12 +94,8 @@ class AnySubscriptionCallback template< typename CallbackT, typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - ConstSharedPtrCallback - >::value - >::type * = nullptr - > + rclcpp::function_traits::same_arguments::value>::type * = + nullptr> void set(CallbackT callback) { const_shared_ptr_callback_ = callback; @@ -113,12 +104,8 @@ class AnySubscriptionCallback template< typename CallbackT, typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - ConstSharedPtrWithInfoCallback - >::value - >::type * = nullptr - > + rclcpp::function_traits::same_arguments::value>:: + type * = nullptr> void set(CallbackT callback) { const_shared_ptr_with_info_callback_ = callback; @@ -127,12 +114,8 @@ class AnySubscriptionCallback template< typename CallbackT, typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - UniquePtrCallback - >::value - >::type * = nullptr - > + rclcpp::function_traits::same_arguments::value>::type * = + nullptr> void set(CallbackT callback) { unique_ptr_callback_ = callback; @@ -141,19 +124,14 @@ class AnySubscriptionCallback template< typename CallbackT, typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - UniquePtrWithInfoCallback - >::value - >::type * = nullptr - > + rclcpp::function_traits::same_arguments::value>:: + type * = nullptr> void set(CallbackT callback) { unique_ptr_with_info_callback_ = callback; } - void dispatch( - std::shared_ptr message, const rmw_message_info_t & message_info) + void dispatch(std::shared_ptr message, const rmw_message_info_t & message_info) { if (shared_ptr_callback_) { shared_ptr_callback_(message); @@ -185,20 +163,18 @@ class AnySubscriptionCallback const_shared_ptr_with_info_callback_(message, message_info); } else { if ( - unique_ptr_callback_ || unique_ptr_with_info_callback_ || - shared_ptr_callback_ || shared_ptr_with_info_callback_) - { + unique_ptr_callback_ || unique_ptr_with_info_callback_ || shared_ptr_callback_ || + shared_ptr_with_info_callback_) { throw std::runtime_error( - "unexpected dispatch_intra_process const shared " - "message call with no const shared_ptr callback"); + "unexpected dispatch_intra_process const shared " + "message call with no const shared_ptr callback"); } else { throw std::runtime_error("unexpected message without any callback set"); } } } - void dispatch_intra_process( - MessageUniquePtr message, const rmw_message_info_t & message_info) + void dispatch_intra_process(MessageUniquePtr message, const rmw_message_info_t & message_info) { if (shared_ptr_callback_) { typename std::shared_ptr shared_message = std::move(message); @@ -212,8 +188,8 @@ class AnySubscriptionCallback unique_ptr_with_info_callback_(std::move(message), message_info); } else if (const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_) { throw std::runtime_error( - "unexpected dispatch_intra_process unique message call" - " with const shared_ptr callback"); + "unexpected dispatch_intra_process unique message call" + " with const shared_ptr callback"); } else { throw std::runtime_error("unexpected message without any callback set"); } diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index a1b7a1229f..5989bbcfb3 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -43,11 +43,7 @@ class NodeWaitables; namespace callback_group { -enum class CallbackGroupType -{ - MutuallyExclusive, - Reentrant -}; +enum class CallbackGroupType { MutuallyExclusive, Reentrant }; class CallbackGroup { @@ -63,78 +59,64 @@ class CallbackGroup explicit CallbackGroup(CallbackGroupType group_type); template - rclcpp::SubscriptionBase::SharedPtr - find_subscription_ptrs_if(Function func) const + rclcpp::SubscriptionBase::SharedPtr find_subscription_ptrs_if(Function func) const { return _find_ptrs_if_impl(func, subscription_ptrs_); } template - rclcpp::TimerBase::SharedPtr - find_timer_ptrs_if(Function func) const + rclcpp::TimerBase::SharedPtr find_timer_ptrs_if(Function func) const { return _find_ptrs_if_impl(func, timer_ptrs_); } template - rclcpp::ServiceBase::SharedPtr - find_service_ptrs_if(Function func) const + rclcpp::ServiceBase::SharedPtr find_service_ptrs_if(Function func) const { return _find_ptrs_if_impl(func, service_ptrs_); } template - rclcpp::ClientBase::SharedPtr - find_client_ptrs_if(Function func) const + rclcpp::ClientBase::SharedPtr find_client_ptrs_if(Function func) const { return _find_ptrs_if_impl(func, client_ptrs_); } template - rclcpp::Waitable::SharedPtr - find_waitable_ptrs_if(Function func) const + rclcpp::Waitable::SharedPtr find_waitable_ptrs_if(Function func) const { return _find_ptrs_if_impl(func, waitable_ptrs_); } RCLCPP_PUBLIC - std::atomic_bool & - can_be_taken_from(); + std::atomic_bool & can_be_taken_from(); RCLCPP_PUBLIC - const CallbackGroupType & - type() const; + const CallbackGroupType & type() const; protected: RCLCPP_DISABLE_COPY(CallbackGroup) RCLCPP_PUBLIC - void - add_publisher(const rclcpp::PublisherBase::SharedPtr publisher_ptr); + void add_publisher(const rclcpp::PublisherBase::SharedPtr publisher_ptr); RCLCPP_PUBLIC - void - add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr); + void add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr); RCLCPP_PUBLIC - void - add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr); + void add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr); RCLCPP_PUBLIC - void - add_service(const rclcpp::ServiceBase::SharedPtr service_ptr); + void add_service(const rclcpp::ServiceBase::SharedPtr service_ptr); RCLCPP_PUBLIC - void - add_client(const rclcpp::ClientBase::SharedPtr client_ptr); + void add_client(const rclcpp::ClientBase::SharedPtr client_ptr); RCLCPP_PUBLIC - void - add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr); + void add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr); RCLCPP_PUBLIC - void - remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept; + void remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept; CallbackGroupType type_; // Mutex to protect the subsequent vectors of pointers. diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 46467400fe..230fde1229 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -28,12 +28,12 @@ #include "rcl/wait.h" #include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" #include "rcutils/logging_macros.h" @@ -63,29 +63,23 @@ class ClientBase virtual ~ClientBase(); RCLCPP_PUBLIC - const char * - get_service_name() const; + const char * get_service_name() const; RCLCPP_PUBLIC - std::shared_ptr - get_client_handle(); + std::shared_ptr get_client_handle(); RCLCPP_PUBLIC - std::shared_ptr - get_client_handle() const; + std::shared_ptr get_client_handle() const; RCLCPP_PUBLIC - bool - service_is_ready() const; + bool service_is_ready() const; template - bool - wait_for_service( + bool wait_for_service( std::chrono::duration timeout = std::chrono::duration(-1)) { return wait_for_service_nanoseconds( - std::chrono::duration_cast(timeout) - ); + std::chrono::duration_cast(timeout)); } virtual std::shared_ptr create_response() = 0; @@ -97,16 +91,13 @@ class ClientBase RCLCPP_DISABLE_COPY(ClientBase) RCLCPP_PUBLIC - bool - wait_for_service_nanoseconds(std::chrono::nanoseconds timeout); + bool wait_for_service_nanoseconds(std::chrono::nanoseconds timeout); RCLCPP_PUBLIC - rcl_node_t * - get_rcl_node_handle(); + rcl_node_t * get_rcl_node_handle(); RCLCPP_PUBLIC - const rcl_node_t * - get_rcl_node_handle() const; + const rcl_node_t * get_rcl_node_handle() const; rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; std::shared_ptr node_handle_; @@ -131,8 +122,8 @@ class Client : public ClientBase using SharedFuture = std::shared_future; using SharedFutureWithRequest = std::shared_future>; - using CallbackType = std::function; - using CallbackWithRequestType = std::function; + using CallbackType = std::function; + using CallbackWithRequestType = std::function; RCLCPP_SMART_PTR_DEFINITIONS(Client) @@ -144,8 +135,7 @@ class Client : public ClientBase : ClientBase(node_base, node_graph) { using rosidl_typesupport_cpp::get_service_type_support_handle; - auto service_type_support_handle = - get_service_type_support_handle(); + auto service_type_support_handle = get_service_type_support_handle(); rcl_ret_t ret = rcl_client_init( this->get_client_handle().get(), this->get_rcl_node_handle(), @@ -171,33 +161,27 @@ class Client : public ClientBase { } - std::shared_ptr - create_response() override + std::shared_ptr create_response() override { return std::shared_ptr(new typename ServiceT::Response()); } - std::shared_ptr - create_request_header() override + std::shared_ptr create_request_header() override { // TODO(wjwwood): This should probably use rmw_request_id's allocator. // (since it is a C type) return std::shared_ptr(new rmw_request_id_t); } - void - handle_response( - std::shared_ptr request_header, - std::shared_ptr response) override + void handle_response( + std::shared_ptr request_header, std::shared_ptr response) override { std::unique_lock lock(pending_requests_mutex_); auto typed_response = std::static_pointer_cast(response); int64_t sequence_number = request_header->sequence_number; // TODO(esteve) this should throw instead since it is not expected to happen in the first place if (this->pending_requests_.count(sequence_number) == 0) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Received invalid sequence number. Ignoring..."); + RCUTILS_LOG_ERROR_NAMED("rclcpp", "Received invalid sequence number. Ignoring..."); return; } auto tuple = this->pending_requests_[sequence_number]; @@ -212,8 +196,7 @@ class Client : public ClientBase callback(future); } - SharedFuture - async_send_request(SharedRequest request) + SharedFuture async_send_request(SharedRequest request) { return async_send_request(request, [](SharedFuture) {}); } @@ -221,14 +204,8 @@ class Client : public ClientBase template< typename CallbackT, typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - CallbackType - >::value - >::type * = nullptr - > - SharedFuture - async_send_request(SharedRequest request, CallbackT && cb) + rclcpp::function_traits::same_arguments::value>::type * = nullptr> + SharedFuture async_send_request(SharedRequest request, CallbackT && cb) { std::lock_guard lock(pending_requests_mutex_); int64_t sequence_number; @@ -247,23 +224,18 @@ class Client : public ClientBase template< typename CallbackT, typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - CallbackWithRequestType - >::value - >::type * = nullptr - > - SharedFutureWithRequest - async_send_request(SharedRequest request, CallbackT && cb) + rclcpp::function_traits::same_arguments::value>::type * = + nullptr> + SharedFutureWithRequest async_send_request(SharedRequest request, CallbackT && cb) { SharedPromiseWithRequest promise = std::make_shared(); SharedFutureWithRequest future_with_request(promise->get_future()); auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) { - auto response = future.get(); - promise->set_value(std::make_pair(request, response)); - cb(future_with_request); - }; + auto response = future.get(); + promise->set_value(std::make_pair(request, response)); + cb(future_with_request); + }; async_send_request(request, wrapping_cb); diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index b9bbe2d41a..c0007030c8 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -33,8 +33,8 @@ class JumpHandler public: RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler) - using pre_callback_t = std::function; - using post_callback_t = std::function; + using pre_callback_t = std::function; + using post_callback_t = std::function; JumpHandler( pre_callback_t pre_callback, @@ -71,8 +71,7 @@ class Clock * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. */ RCLCPP_PUBLIC - Time - now(); + Time now(); /** * Returns the clock of the type `RCL_ROS_TIME` is active. @@ -82,16 +81,13 @@ class Clock * the current clock does not have the clock_type `RCL_ROS_TIME`. */ RCLCPP_PUBLIC - bool - ros_time_is_active(); + bool ros_time_is_active(); RCLCPP_PUBLIC - rcl_clock_t * - get_clock_handle() noexcept; + rcl_clock_t * get_clock_handle() noexcept; RCLCPP_PUBLIC - rcl_clock_type_t - get_clock_type() const noexcept; + rcl_clock_type_t get_clock_type() const noexcept; // Add a callback to invoke if the jump threshold is exceeded. /** @@ -115,8 +111,7 @@ class Clock * JumpHandler. */ RCLCPP_PUBLIC - JumpHandler::SharedPtr - create_jump_callback( + JumpHandler::SharedPtr create_jump_callback( JumpHandler::pre_callback_t pre_callback, JumpHandler::post_callback_t post_callback, const rcl_jump_threshold_t & threshold); @@ -124,11 +119,8 @@ class Clock private: // Invoke time jump callback RCLCPP_PUBLIC - static void - on_time_jump( - const struct rcl_time_jump_t * time_jump, - bool before_jump, - void * user_data); + static void on_time_jump( + const struct rcl_time_jump_t * time_jump, bool before_jump, void * user_data); /// Internal storage backed by rcl rcl_clock_t rcl_clock_; diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index dfc33d5e4f..866fef6da9 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -40,8 +40,9 @@ namespace rclcpp class ContextAlreadyInitialized : public std::runtime_error { public: - ContextAlreadyInitialized() - : std::runtime_error("context is already initialized") {} + ContextAlreadyInitialized() : std::runtime_error("context is already initialized") + { + } }; /// Context which encapsulates shared state between nodes and other similar entities. @@ -66,8 +67,7 @@ class Context : public std::enable_shared_from_this Context(); RCLCPP_PUBLIC - virtual - ~Context(); + virtual ~Context(); /// Initialize the context, and the underlying elements like the rcl context. /** @@ -102,9 +102,7 @@ class Context : public std::enable_shared_from_this * \throw ContextAlreadyInitialized if called if init is called more than once */ RCLCPP_PUBLIC - virtual - void - init( + virtual void init( int argc, char const * const argv[], const rclcpp::InitOptions & init_options = rclcpp::InitOptions()); @@ -120,26 +118,22 @@ class Context : public std::enable_shared_from_this * \return true if valid, otherwise false */ RCLCPP_PUBLIC - bool - is_valid() const; + bool is_valid() const; /// Return the init options used during init. RCLCPP_PUBLIC - const rclcpp::InitOptions & - get_init_options() const; + const rclcpp::InitOptions & get_init_options() const; /// Return a copy of the init options used during init. RCLCPP_PUBLIC - rclcpp::InitOptions - get_init_options(); + rclcpp::InitOptions get_init_options(); /// Return the shutdown reason, or empty string if not shutdown. /** * This function is thread-safe. */ RCLCPP_PUBLIC - std::string - shutdown_reason(); + std::string shutdown_reason(); /// Shutdown the context, making it uninitialized and therefore invalid for derived entities. /** @@ -162,11 +156,9 @@ class Context : public std::enable_shared_from_this * \throw various exceptions derived from RCLErrorBase, if rcl_shutdown fails */ RCLCPP_PUBLIC - virtual - bool - shutdown(const std::string & reason); + virtual bool shutdown(const std::string & reason); - using OnShutdownCallback = std::function; + using OnShutdownCallback = std::function; /// Add a on_shutdown callback to be called when shutdown is called for this context. /** @@ -188,9 +180,7 @@ class Context : public std::enable_shared_from_this * \return the callback passed, for convenience when storing a passed lambda */ RCLCPP_PUBLIC - virtual - OnShutdownCallback - on_shutdown(OnShutdownCallback callback); + virtual OnShutdownCallback on_shutdown(OnShutdownCallback callback); /// Return the shutdown callbacks as const. /** @@ -198,8 +188,7 @@ class Context : public std::enable_shared_from_this * the list of "on shutdown" callbacks, i.e. on_shutdown(). */ RCLCPP_PUBLIC - const std::vector & - get_on_shutdown_callbacks() const; + const std::vector & get_on_shutdown_callbacks() const; /// Return the shutdown callbacks. /** @@ -207,13 +196,11 @@ class Context : public std::enable_shared_from_this * the list of "on shutdown" callbacks, i.e. on_shutdown(). */ RCLCPP_PUBLIC - std::vector & - get_on_shutdown_callbacks(); + std::vector & get_on_shutdown_callbacks(); /// Return the internal rcl context. RCLCPP_PUBLIC - std::shared_ptr - get_rcl_context(); + std::shared_ptr get_rcl_context(); /// Sleep for a given period of time or until shutdown() is called. /** @@ -228,14 +215,11 @@ class Context : public std::enable_shared_from_this * \return true if the condition variable did not timeout, i.e. you were interrupted. */ RCLCPP_PUBLIC - bool - sleep_for(const std::chrono::nanoseconds & nanoseconds); + bool sleep_for(const std::chrono::nanoseconds & nanoseconds); /// Interrupt any blocking sleep_for calls, causing them to return immediately and return true. RCLCPP_PUBLIC - virtual - void - interrupt_all_sleep_for(); + virtual void interrupt_all_sleep_for(); /// Get a handle to the guard condition which is triggered when interrupted. /** @@ -262,8 +246,7 @@ class Context : public std::enable_shared_from_this * \return Pointer to the guard condition. */ RCLCPP_PUBLIC - rcl_guard_condition_t * - get_interrupt_guard_condition(rcl_wait_set_t * wait_set); + rcl_guard_condition_t * get_interrupt_guard_condition(rcl_wait_set_t * wait_set); /// Release the previously allocated guard condition which is triggered when interrupted. /** @@ -281,24 +264,20 @@ class Context : public std::enable_shared_from_this * resulting guard condition. */ RCLCPP_PUBLIC - void - release_interrupt_guard_condition(rcl_wait_set_t * wait_set); + void release_interrupt_guard_condition(rcl_wait_set_t * wait_set); /// Nothrow version of release_interrupt_guard_condition(), logs to RCLCPP_ERROR instead. RCLCPP_PUBLIC - void - release_interrupt_guard_condition(rcl_wait_set_t * wait_set, const std::nothrow_t &) noexcept; + void release_interrupt_guard_condition( + rcl_wait_set_t * wait_set, const std::nothrow_t &) noexcept; /// Interrupt any blocking executors, or wait sets associated with this context. RCLCPP_PUBLIC - virtual - void - interrupt_all_wait_sets(); + virtual void interrupt_all_wait_sets(); /// Return a singleton instance for the SubContext type, constructing one if necessary. - template - std::shared_ptr - get_sub_context(Args && ... args) + template + std::shared_ptr get_sub_context(Args &&... args) { std::lock_guard lock(sub_contexts_mutex_); @@ -308,10 +287,8 @@ class Context : public std::enable_shared_from_this if (it == sub_contexts_.end()) { // It doesn't exist yet, make it sub_context = std::shared_ptr( - new SubContext(std::forward(args) ...), - [](SubContext * sub_context_ptr) { - delete sub_context_ptr; - }); + new SubContext(std::forward(args)...), + [](SubContext * sub_context_ptr) { delete sub_context_ptr; }); sub_contexts_[type_i] = sub_context; } else { // It exists, get it out and cast it. @@ -324,9 +301,7 @@ class Context : public std::enable_shared_from_this // Called by constructor and destructor to clean up by finalizing the // shutdown rcl context and preparing for a new init cycle. RCLCPP_PUBLIC - virtual - void - clean_up(); + virtual void clean_up(); private: RCLCPP_DISABLE_COPY(Context) @@ -362,8 +337,7 @@ class Context : public std::enable_shared_from_this * This function is thread-safe. */ RCLCPP_PUBLIC -std::vector -get_contexts(); +std::vector get_contexts(); } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/contexts/default_context.hpp b/rclcpp/include/rclcpp/contexts/default_context.hpp index 2ed9bcbbf1..f6d924bd98 100644 --- a/rclcpp/include/rclcpp/contexts/default_context.hpp +++ b/rclcpp/include/rclcpp/contexts/default_context.hpp @@ -35,8 +35,7 @@ class DefaultContext : public rclcpp::Context }; RCLCPP_PUBLIC -DefaultContext::SharedPtr -get_global_default_context(); +DefaultContext::SharedPtr get_global_default_context(); } // namespace default_context } // namespace contexts diff --git a/rclcpp/include/rclcpp/create_client.hpp b/rclcpp/include/rclcpp/create_client.hpp index 94f6b212fa..e1fe3ea1b5 100644 --- a/rclcpp/include/rclcpp/create_client.hpp +++ b/rclcpp/include/rclcpp/create_client.hpp @@ -28,8 +28,7 @@ namespace rclcpp /// Create a service client with a given type. /// \internal template -typename rclcpp::Client::SharedPtr -create_client( +typename rclcpp::Client::SharedPtr create_client( std::shared_ptr node_base, std::shared_ptr node_graph, std::shared_ptr node_services, @@ -40,11 +39,8 @@ create_client( rcl_client_options_t options = rcl_client_get_default_options(); options.qos = qos_profile; - auto cli = rclcpp::Client::make_shared( - node_base.get(), - node_graph, - service_name, - options); + auto cli = + rclcpp::Client::make_shared(node_base.get(), node_graph, service_name, options); auto cli_base_ptr = std::dynamic_pointer_cast(cli); node_services->add_client(cli_base_ptr, group); diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index 0e99c374e5..81fecb7dc0 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -39,14 +39,12 @@ template< typename AllocatorT = std::allocator, typename PublisherT = ::rclcpp::Publisher, typename NodeT> -std::shared_ptr -create_publisher( +std::shared_ptr create_publisher( NodeT & node, const std::string & topic_name, const rclcpp::QoS & qos, - const rclcpp::PublisherOptionsWithAllocator & options = ( - rclcpp::PublisherOptionsWithAllocator() -)) + const rclcpp::PublisherOptionsWithAllocator & options = + (rclcpp::PublisherOptionsWithAllocator())) { using rclcpp::node_interfaces::get_node_topics_interface; auto node_topics = get_node_topics_interface(node); @@ -76,12 +74,9 @@ create_publisher( auto pub = node_topics->create_publisher( topic_name, rclcpp::create_publisher_factory( - options.event_callbacks, - allocator - ), + options.event_callbacks, allocator), options.template to_rcl_publisher_options(qos), - use_intra_process - ); + use_intra_process); node_topics->add_publisher(pub, options.callback_group); return std::dynamic_pointer_cast(pub); } diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp index ac7846239d..07cee03783 100644 --- a/rclcpp/include/rclcpp/create_service.hpp +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -30,8 +30,7 @@ namespace rclcpp /// Create a service with a given type. /// \internal template -typename rclcpp::Service::SharedPtr -create_service( +typename rclcpp::Service::SharedPtr create_service( std::shared_ptr node_base, std::shared_ptr node_services, const std::string & service_name, @@ -46,8 +45,7 @@ create_service( service_options.qos = qos_profile; auto serv = Service::make_shared( - node_base->get_shared_rcl_node_handle(), - service_name, any_service_callback, service_options); + node_base->get_shared_rcl_node_handle(), service_name, any_service_callback, service_options); auto serv_base_ptr = std::dynamic_pointer_cast(serv); node_services->add_service(serv_base_ptr, group); return serv; diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 7c13d6f643..ea06d86177 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -21,9 +21,9 @@ #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/subscription_factory.hpp" #include "rclcpp/subscription_options.hpp" -#include "rclcpp/qos.hpp" #include "rmw/qos_profiles.h" namespace rclcpp @@ -40,21 +40,18 @@ template< typename CallbackT, typename AllocatorT = std::allocator, typename CallbackMessageT = - typename rclcpp::subscription_traits::has_message_type::type, + typename rclcpp::subscription_traits::has_message_type::type, typename SubscriptionT = rclcpp::Subscription, typename NodeT> -typename std::shared_ptr -create_subscription( +typename std::shared_ptr create_subscription( NodeT && node, const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, - const rclcpp::SubscriptionOptionsWithAllocator & options = ( - rclcpp::SubscriptionOptionsWithAllocator() - ), - typename rclcpp::message_memory_strategy::MessageMemoryStrategy< - CallbackMessageT, AllocatorT>::SharedPtr - msg_mem_strat = nullptr) + const rclcpp::SubscriptionOptionsWithAllocator & options = + (rclcpp::SubscriptionOptionsWithAllocator()), + typename rclcpp::message_memory_strategy::MessageMemoryStrategy:: + SharedPtr msg_mem_strat = nullptr) { using rclcpp::node_interfaces::get_node_topics_interface; auto node_topics = get_node_topics_interface(std::forward(node)); @@ -68,9 +65,9 @@ create_subscription( if (!allocator) { allocator = std::make_shared(); } - auto factory = rclcpp::create_subscription_factory - ( - std::forward(callback), options.event_callbacks, msg_mem_strat, allocator); + auto factory = rclcpp:: + create_subscription_factory( + std::forward(callback), options.event_callbacks, msg_mem_strat, allocator); bool use_intra_process; switch (options.use_intra_process_comm) { diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 208474f345..ac7b60898d 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -30,8 +30,7 @@ namespace rclcpp /// Create a timer with a given clock /// \internal template -typename rclcpp::TimerBase::SharedPtr -create_timer( +typename rclcpp::TimerBase::SharedPtr create_timer( node_interfaces::NodeBaseInterface * node_base, node_interfaces::NodeTimersInterface * node_timers, rclcpp::Clock::SharedPtr clock, @@ -51,8 +50,7 @@ create_timer( /// Create a timer with a given clock template -typename rclcpp::TimerBase::SharedPtr -create_timer( +typename rclcpp::TimerBase::SharedPtr create_timer( NodeT node, rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index 5e09cc5cb1..6de43c87d3 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -39,7 +39,8 @@ class RCLCPP_PUBLIC Duration // cppcheck-suppress noExplicitConstructor Duration(const std::chrono::duration & duration) // NOLINT(runtime/explicit) : Duration(std::chrono::duration_cast(duration)) - {} + { + } // cppcheck-suppress noExplicitConstructor Duration(const builtin_interfaces::msg::Duration & duration_msg); // NOLINT(runtime/explicit) @@ -53,62 +54,45 @@ class RCLCPP_PUBLIC Duration operator builtin_interfaces::msg::Duration() const; // cppcheck-suppress operatorEq // this is a false positive from cppcheck - Duration & - operator=(const Duration & rhs); + Duration & operator=(const Duration & rhs); - Duration & - operator=(const builtin_interfaces::msg::Duration & Duration_msg); + Duration & operator=(const builtin_interfaces::msg::Duration & Duration_msg); - bool - operator==(const rclcpp::Duration & rhs) const; + bool operator==(const rclcpp::Duration & rhs) const; - bool - operator<(const rclcpp::Duration & rhs) const; + bool operator<(const rclcpp::Duration & rhs) const; - bool - operator<=(const rclcpp::Duration & rhs) const; + bool operator<=(const rclcpp::Duration & rhs) const; - bool - operator>=(const rclcpp::Duration & rhs) const; + bool operator>=(const rclcpp::Duration & rhs) const; - bool - operator>(const rclcpp::Duration & rhs) const; + bool operator>(const rclcpp::Duration & rhs) const; - Duration - operator+(const rclcpp::Duration & rhs) const; + Duration operator+(const rclcpp::Duration & rhs) const; - Duration - operator-(const rclcpp::Duration & rhs) const; + Duration operator-(const rclcpp::Duration & rhs) const; - static - Duration - max(); + static Duration max(); - Duration - operator*(double scale) const; + Duration operator*(double scale) const; - rcl_duration_value_t - nanoseconds() const; + rcl_duration_value_t nanoseconds() const; /// \return the duration in seconds as a floating point number. /// \warning Depending on sizeof(double) there could be significant precision loss. /// When an exact time is required use nanoseconds() instead. - double - seconds() const; + double seconds() const; // Create a duration object from a floating point number representing seconds - static Duration - from_seconds(double seconds); + static Duration from_seconds(double seconds); template - DurationT - to_chrono() const + DurationT to_chrono() const { return std::chrono::duration_cast(std::chrono::nanoseconds(this->nanoseconds())); } - rmw_time_t - to_rmw_time() const; + rmw_time_t to_rmw_time() const; private: rcl_duration_t rcl_duration_; diff --git a/rclcpp/include/rclcpp/event.hpp b/rclcpp/include/rclcpp/event.hpp index 988dba29e2..be90a042ba 100644 --- a/rclcpp/include/rclcpp/event.hpp +++ b/rclcpp/include/rclcpp/event.hpp @@ -33,16 +33,13 @@ class Event Event(); RCLCPP_PUBLIC - bool - set(); + bool set(); RCLCPP_PUBLIC - bool - check(); + bool check(); RCLCPP_PUBLIC - bool - check_and_clear(); + bool check_and_clear(); private: RCLCPP_DISABLE_COPY(Event) diff --git a/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp/include/rclcpp/exceptions.hpp index 1976902d92..00ec77f718 100644 --- a/rclcpp/include/rclcpp/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions.hpp @@ -34,8 +34,9 @@ namespace exceptions class InvalidNodeError : public std::runtime_error { public: - InvalidNodeError() - : std::runtime_error("node is invalid") {} + InvalidNodeError() : std::runtime_error("node is invalid") + { + } }; /// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid. @@ -43,20 +44,17 @@ class NameValidationError : public std::invalid_argument { public: NameValidationError( - const char * name_type_, - const char * name_, - const char * error_msg_, - size_t invalid_index_) + const char * name_type_, const char * name_, const char * error_msg_, size_t invalid_index_) : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)), - name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_) - {} + name_type(name_type_), + name(name_), + error_msg(error_msg_), + invalid_index(invalid_index_) + { + } - static std::string - format_error( - const char * name_type, - const char * name, - const char * error_msg, - size_t invalid_index); + static std::string format_error( + const char * name_type, const char * name, const char * error_msg, size_t invalid_index); const std::string name_type; const std::string name; @@ -70,7 +68,8 @@ class InvalidNodeNameError : public NameValidationError public: InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index) : NameValidationError("node name", node_name, error_msg, invalid_index) - {} + { + } }; /// Thrown when a node namespace is invalid. @@ -79,7 +78,8 @@ class InvalidNamespaceError : public NameValidationError public: InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index) : NameValidationError("namespace", namespace_, error_msg, invalid_index) - {} + { + } }; /// Thrown when a topic name is invalid. @@ -88,7 +88,8 @@ class InvalidTopicNameError : public NameValidationError public: InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index) : NameValidationError("topic name", namespace_, error_msg, invalid_index) - {} + { + } }; /// Thrown when a service name is invalid. @@ -97,7 +98,8 @@ class InvalidServiceNameError : public NameValidationError public: InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index) : NameValidationError("service name", namespace_, error_msg, invalid_index) - {} + { + } }; /// Throw a C++ std::exception which was created based on an rcl error. @@ -115,12 +117,11 @@ class InvalidServiceNameError : public NameValidationError */ /* *INDENT-OFF* */ // Uncrustify cannot yet understand [[noreturn]] properly RCLCPP_PUBLIC -void -throw_from_rcl_error [[noreturn]] ( +void throw_from_rcl_error[[noreturn]]( rcl_ret_t ret, const std::string & prefix = "", const rcl_error_state_t * error_state = nullptr, - void (* reset_error)() = rcl_reset_error); + void (*reset_error)() = rcl_reset_error); /* *INDENT-ON* */ class RCLErrorBase @@ -128,7 +129,9 @@ class RCLErrorBase public: RCLCPP_PUBLIC RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state); - virtual ~RCLErrorBase() {} + virtual ~RCLErrorBase() + { + } rcl_ret_t ret; std::string message; @@ -163,9 +166,7 @@ class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument public: RCLCPP_PUBLIC RCLInvalidArgument( - rcl_ret_t ret, - const rcl_error_state_t * error_state, - const std::string & prefix); + rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix); RCLCPP_PUBLIC RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix); }; @@ -199,16 +200,18 @@ class UnknownROSArgsError : public std::runtime_error class InvalidEventError : public std::runtime_error { public: - InvalidEventError() - : std::runtime_error("event is invalid") {} + InvalidEventError() : std::runtime_error("event is invalid") + { + } }; /// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected. class EventNotRegisteredError : public std::runtime_error { public: - EventNotRegisteredError() - : std::runtime_error("event already registered") {} + EventNotRegisteredError() : std::runtime_error("event already registered") + { + } }; /// Thrown if passed parameters are inconsistent or invalid diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 095a765e86..ea207354b3 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -52,15 +52,13 @@ namespace executor * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error. * TIMEOUT: Spinning timed out. */ -enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; +enum class FutureReturnCode { SUCCESS, INTERRUPTED, TIMEOUT }; RCLCPP_PUBLIC -std::ostream & -operator<<(std::ostream & os, const FutureReturnCode & future_return_code); +std::ostream & operator<<(std::ostream & os, const FutureReturnCode & future_return_code); RCLCPP_PUBLIC -std::string -to_string(const FutureReturnCode & future_return_code); +std::string to_string(const FutureReturnCode & future_return_code); /// /** @@ -72,7 +70,8 @@ struct ExecutorArgs : memory_strategy(memory_strategies::create_default_strategy()), context(rclcpp::contexts::default_context::get_global_default_context()), max_conditions(0) - {} + { + } memory_strategy::MemoryStrategy::SharedPtr memory_strategy; std::shared_ptr context; @@ -110,8 +109,7 @@ class Executor /// Do work periodically as it becomes available to us. Blocking call, may block indefinitely. // It is up to the implementation of Executor to implement spin. - virtual void - spin() = 0; + virtual void spin() = 0; /// Add a node to the executor. /** @@ -122,13 +120,12 @@ class Executor * node was added, it will wake up. */ RCLCPP_PUBLIC - virtual void - add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); + virtual void add_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); /// Convenience function which takes Node and forwards NodeBaseInterface. RCLCPP_PUBLIC - virtual void - add_node(std::shared_ptr node_ptr, bool notify = true); + virtual void add_node(std::shared_ptr node_ptr, bool notify = true); /// Remove a node from the executor. /** @@ -138,13 +135,12 @@ class Executor * waiting for work in another thread, because otherwise the executor would never be notified. */ RCLCPP_PUBLIC - virtual void - remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); + virtual void remove_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); /// Convenience function which takes Node and forwards NodeBaseInterface. RCLCPP_PUBLIC - virtual void - remove_node(std::shared_ptr node_ptr, bool notify = true); + virtual void remove_node(std::shared_ptr node_ptr, bool notify = true); /// Add a node to executor, execute the next available unit of work, and remove the node. /** @@ -154,28 +150,23 @@ class Executor * function to be non-blocking. */ template - void - spin_node_once( + void spin_node_once( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, std::chrono::duration timeout = std::chrono::duration(-1)) { return spin_node_once_nanoseconds( - node, - std::chrono::duration_cast(timeout) - ); + node, std::chrono::duration_cast(timeout)); } /// Convenience function which takes Node and forwards NodeBaseInterface. template - void - spin_node_once( + void spin_node_once( std::shared_ptr node, std::chrono::duration timeout = std::chrono::duration(-1)) { return spin_node_once_nanoseconds( node->get_node_base_interface(), - std::chrono::duration_cast(timeout) - ); + std::chrono::duration_cast(timeout)); } /// Add a node, complete all immediately available work, and remove the node. @@ -183,13 +174,11 @@ class Executor * \param[in] node Shared pointer to the node to add. */ RCLCPP_PUBLIC - void - spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node); + void spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node); /// Convenience function which takes Node and forwards NodeBaseInterface. RCLCPP_PUBLIC - void - spin_node_some(std::shared_ptr node); + void spin_node_some(std::shared_ptr node); /// Complete all available queued work without blocking. /** @@ -203,12 +192,10 @@ class Executor * been exceeded. */ RCLCPP_PUBLIC - virtual void - spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)); + virtual void spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)); RCLCPP_PUBLIC - virtual void - spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + virtual void spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. /** @@ -221,8 +208,7 @@ class Executor * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ template - FutureReturnCode - spin_until_future_complete( + FutureReturnCode spin_until_future_complete( std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) { @@ -237,8 +223,8 @@ class Executor } auto end_time = std::chrono::steady_clock::now(); - std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( - timeout); + std::chrono::nanoseconds timeout_ns = + std::chrono::duration_cast(timeout); if (timeout_ns > std::chrono::nanoseconds::zero()) { end_time += timeout_ns; } @@ -272,8 +258,7 @@ class Executor /// Cancel any running spin* function, causing it to return. /* This function can be called asynchonously from any thread. */ RCLCPP_PUBLIC - void - cancel(); + void cancel(); /// Support dynamic switching of the memory strategy. /** @@ -282,65 +267,51 @@ class Executor * \param[in] memory_strategy Shared pointer to the memory strategy to set. */ RCLCPP_PUBLIC - void - set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy); + void set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy); protected: RCLCPP_PUBLIC - void - spin_node_once_nanoseconds( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, - std::chrono::nanoseconds timeout); + void spin_node_once_nanoseconds( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, std::chrono::nanoseconds timeout); /// Find the next available executable and do the work associated with it. /** \param[in] any_exec Union structure that can hold any executable type (timer, subscription, * service, client). */ RCLCPP_PUBLIC - void - execute_any_executable(AnyExecutable & any_exec); + void execute_any_executable(AnyExecutable & any_exec); RCLCPP_PUBLIC - static void - execute_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription); + static void execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription); RCLCPP_PUBLIC - static void - execute_intra_process_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription); + static void execute_intra_process_subscription(rclcpp::SubscriptionBase::SharedPtr subscription); RCLCPP_PUBLIC - static void - execute_timer(rclcpp::TimerBase::SharedPtr timer); + static void execute_timer(rclcpp::TimerBase::SharedPtr timer); RCLCPP_PUBLIC - static void - execute_service(rclcpp::ServiceBase::SharedPtr service); + static void execute_service(rclcpp::ServiceBase::SharedPtr service); RCLCPP_PUBLIC - static void - execute_client(rclcpp::ClientBase::SharedPtr client); + static void execute_client(rclcpp::ClientBase::SharedPtr client); RCLCPP_PUBLIC - void - wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + void wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group( + rclcpp::callback_group::CallbackGroup::SharedPtr group); RCLCPP_PUBLIC - rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_timer(rclcpp::TimerBase::SharedPtr timer); + rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_timer( + rclcpp::TimerBase::SharedPtr timer); RCLCPP_PUBLIC - bool - get_next_ready_executable(AnyExecutable & any_executable); + bool get_next_ready_executable(AnyExecutable & any_executable); RCLCPP_PUBLIC - bool - get_next_executable( + bool get_next_executable( AnyExecutable & any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 7417098b1c..6dee2e2570 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -30,22 +30,18 @@ namespace rclcpp /// Create a default single-threaded executor and execute any immediately available work. /** \param[in] node_ptr Shared pointer to the node to spin. */ RCLCPP_PUBLIC -void -spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); +void spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); RCLCPP_PUBLIC -void -spin_some(rclcpp::Node::SharedPtr node_ptr); +void spin_some(rclcpp::Node::SharedPtr node_ptr); /// Create a default single-threaded executor and spin the specified node. /** \param[in] node_ptr Shared pointer to the node to spin. */ RCLCPP_PUBLIC -void -spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); +void spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); RCLCPP_PUBLIC -void -spin(rclcpp::Node::SharedPtr node_ptr); +void spin(rclcpp::Node::SharedPtr node_ptr); namespace executors { @@ -66,8 +62,7 @@ using rclcpp::executors::SingleThreadedExecutor; * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ template -rclcpp::executor::FutureReturnCode -spin_node_until_future_complete( +rclcpp::executor::FutureReturnCode spin_node_until_future_complete( rclcpp::executor::Executor & executor, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, std::shared_future & future, @@ -81,27 +76,25 @@ spin_node_until_future_complete( return retcode; } -template -rclcpp::executor::FutureReturnCode -spin_node_until_future_complete( +rclcpp::executor::FutureReturnCode spin_node_until_future_complete( rclcpp::executor::Executor & executor, std::shared_ptr node_ptr, std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::executors::spin_node_until_future_complete( - executor, - node_ptr->get_node_base_interface(), - future, - timeout); + executor, node_ptr->get_node_base_interface(), future, timeout); } } // namespace executors template -rclcpp::executor::FutureReturnCode -spin_until_future_complete( +rclcpp::executor::FutureReturnCode spin_until_future_complete( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) @@ -110,10 +103,12 @@ spin_until_future_complete( return executors::spin_node_until_future_complete(executor, node_ptr, future, timeout); } -template -rclcpp::executor::FutureReturnCode -spin_until_future_complete( +rclcpp::executor::FutureReturnCode spin_until_future_complete( std::shared_ptr node_ptr, std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 316c016a45..5c02ac5964 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -61,17 +61,14 @@ class MultiThreadedExecutor : public executor::Executor virtual ~MultiThreadedExecutor(); RCLCPP_PUBLIC - void - spin(); + void spin(); RCLCPP_PUBLIC - size_t - get_number_of_threads(); + size_t get_number_of_threads(); protected: RCLCPP_PUBLIC - void - run(size_t this_thread_number); + void run(size_t this_thread_number); private: RCLCPP_DISABLE_COPY(MultiThreadedExecutor) diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp index f6bcfa755c..e5b7ccf6d5 100644 --- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -26,8 +26,8 @@ #include "rclcpp/macros.hpp" #include "rclcpp/memory_strategies.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp/rate.hpp" +#include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -44,8 +44,7 @@ class SingleThreadedExecutor : public executor::Executor /// Default constructor. See the default constructor for Executor. RCLCPP_PUBLIC - SingleThreadedExecutor( - const executor::ExecutorArgs & args = executor::ExecutorArgs()); + SingleThreadedExecutor(const executor::ExecutorArgs & args = executor::ExecutorArgs()); /// Default destrcutor. RCLCPP_PUBLIC @@ -55,8 +54,7 @@ class SingleThreadedExecutor : public executor::Executor // This function will block until work comes in, execute it, and keep blocking. // It will only be interrupt by a CTRL-C (managed by the global signal handler). RCLCPP_PUBLIC - void - spin(); + void spin(); private: RCLCPP_DISABLE_COPY(SingleThreadedExecutor) diff --git a/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp b/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp index 5454dfac87..c512a861b2 100644 --- a/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp +++ b/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp @@ -51,8 +51,7 @@ namespace rclcpp * \throws RCLError if an unexpect error occurs */ RCLCPP_PUBLIC -std::string -expand_topic_or_service_name( +std::string expand_topic_or_service_name( const std::string & name, const std::string & node_name, const std::string & namespace_, diff --git a/rclcpp/include/rclcpp/function_traits.hpp b/rclcpp/include/rclcpp/function_traits.hpp index 3599df5001..f55bb46a8b 100644 --- a/rclcpp/include/rclcpp/function_traits.hpp +++ b/rclcpp/include/rclcpp/function_traits.hpp @@ -38,10 +38,10 @@ namespace function_traits template struct tuple_tail; -template -struct tuple_tail> +template +struct tuple_tail> { - using type = std::tuple; + using type = std::tuple; }; // std::function @@ -49,21 +49,21 @@ template struct function_traits { using arguments = typename tuple_tail< - typename function_traits::arguments>::type; + typename function_traits::arguments>::type; static constexpr std::size_t arity = std::tuple_size::value; template using argument_type = typename std::tuple_element::type; - using return_type = typename function_traits::return_type; + using return_type = typename function_traits::return_type; }; // Free functions -template -struct function_traits +template +struct function_traits { - using arguments = std::tuple; + using arguments = std::tuple; static constexpr std::size_t arity = std::tuple_size::value; @@ -74,95 +74,100 @@ struct function_traits }; // Function pointers -template -struct function_traits: function_traits -{}; +template +struct function_traits : function_traits +{ +}; // std::bind for object methods -template +template #if defined _LIBCPP_VERSION // libc++ (Clang) -struct function_traits> +struct function_traits> #elif defined _GLIBCXX_RELEASE // glibc++ (GNU C++ >= 7.1) -struct function_traits> -#elif defined __GLIBCXX__ // glibc++ (GNU C++) -struct function_traits(FArgs ...)>> -#elif defined _MSC_VER // MS Visual Studio +struct function_traits> +#elif defined __GLIBCXX__ // glibc++ (GNU C++) +struct function_traits(FArgs...)>> +#elif defined _MSC_VER // MS Visual Studio struct function_traits< - std::_Binder -> + std::_Binder> #else #error "Unsupported C++ compiler / standard library" #endif - : function_traits -{}; +: function_traits +{ +}; // std::bind for object const methods -template +template #if defined _LIBCPP_VERSION // libc++ (Clang) -struct function_traits> +struct function_traits> #elif defined _GLIBCXX_RELEASE // glibc++ (GNU C++ >= 7.1) -struct function_traits> -#elif defined __GLIBCXX__ // glibc++ (GNU C++) -struct function_traits(FArgs ...)>> -#elif defined _MSC_VER // MS Visual Studio +struct function_traits> +#elif defined __GLIBCXX__ // glibc++ (GNU C++) +struct function_traits(FArgs...)>> +#elif defined _MSC_VER // MS Visual Studio struct function_traits< - std::_Binder -> + std::_Binder> #else #error "Unsupported C++ compiler / standard library" #endif - : function_traits -{}; +: function_traits +{ +}; // std::bind for free functions -template +template #if defined _LIBCPP_VERSION // libc++ (Clang) -struct function_traits> +struct function_traits> #elif defined __GLIBCXX__ // glibc++ (GNU C++) -struct function_traits> -#elif defined _MSC_VER // MS Visual Studio -struct function_traits> +struct function_traits> +#elif defined _MSC_VER // MS Visual Studio +struct function_traits> #else #error "Unsupported C++ compiler / standard library" #endif - : function_traits -{}; +: function_traits +{ +}; // Lambdas -template -struct function_traits - : function_traits -{}; +template +struct function_traits +: function_traits +{ +}; template -struct function_traits: function_traits -{}; +struct function_traits : function_traits +{ +}; template -struct function_traits: function_traits -{}; +struct function_traits : function_traits +{ +}; /* NOTE(esteve): * VS2015 does not support expression SFINAE, so we're using this template to evaluate * the arity of a function. */ template -struct arity_comparator : std::integral_constant< - bool, (Arity == function_traits::arity)> {}; +struct arity_comparator : std::integral_constant::arity)> +{ +}; -template -struct check_arguments : std::is_same< - typename function_traits::arguments, - std::tuple -> -{}; +template +struct check_arguments +: std::is_same::arguments, std::tuple> +{ +}; template struct same_arguments : std::is_same< - typename function_traits::arguments, - typename function_traits::arguments -> -{}; + typename function_traits::arguments, + typename function_traits::arguments> +{ +}; } // namespace function_traits diff --git a/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp/include/rclcpp/graph_listener.hpp index ca4f05c487..84964a8284 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -38,24 +38,27 @@ namespace graph_listener class GraphListenerShutdownError : public std::runtime_error { public: - GraphListenerShutdownError() - : std::runtime_error("GraphListener already shutdown") {} + GraphListenerShutdownError() : std::runtime_error("GraphListener already shutdown") + { + } }; /// Thrown when a node has already been added to the GraphListener. class NodeAlreadyAddedError : public std::runtime_error { public: - NodeAlreadyAddedError() - : std::runtime_error("node already added") {} + NodeAlreadyAddedError() : std::runtime_error("node already added") + { + } }; /// Thrown when the given node is not in the GraphListener. class NodeNotFoundError : public std::runtime_error { public: - NodeNotFoundError() - : std::runtime_error("node not found") {} + NodeNotFoundError() : std::runtime_error("node not found") + { + } }; /// Notifies many nodes of graph changes by listening in a thread. @@ -75,9 +78,7 @@ class GraphListener : public std::enable_shared_from_this * \throws GraphListenerShutdownError if the GraphListener is shutdown */ RCLCPP_PUBLIC - virtual - void - start_if_not_started(); + virtual void start_if_not_started(); /// Add a node to the graph listener's list of nodes. /** @@ -87,9 +88,7 @@ class GraphListener : public std::enable_shared_from_this * \throws std::system_error anything std::mutex::lock() throws */ RCLCPP_PUBLIC - virtual - void - add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); + virtual void add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); /// Return true if the given node is in the graph listener's list of nodes. /** @@ -98,9 +97,7 @@ class GraphListener : public std::enable_shared_from_this * \throws std::system_error anything std::mutex::lock() throws */ RCLCPP_PUBLIC - virtual - bool - has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); + virtual bool has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); /// Remove a node from the graph listener's list of nodes. /** @@ -110,9 +107,7 @@ class GraphListener : public std::enable_shared_from_this * \throws std::system_error anything std::mutex::lock() throws */ RCLCPP_PUBLIC - virtual - void - remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); + virtual void remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); /// Stop the listening thread. /** @@ -130,40 +125,29 @@ class GraphListener : public std::enable_shared_from_this * \throws std::system_error anything std::mutex::lock() throws */ RCLCPP_PUBLIC - virtual - void - shutdown(); + virtual void shutdown(); /// Nothrow version of shutdown(), logs to RCLCPP_ERROR instead. RCLCPP_PUBLIC - virtual - void - shutdown(const std::nothrow_t &) noexcept; + virtual void shutdown(const std::nothrow_t &) noexcept; /// Return true if shutdown() has been called, else false. RCLCPP_PUBLIC - virtual - bool - is_shutdown(); + virtual bool is_shutdown(); protected: /// Main function for the listening thread. RCLCPP_PUBLIC - virtual - void - run(); + virtual void run(); RCLCPP_PUBLIC - virtual - void - run_loop(); + virtual void run_loop(); private: RCLCPP_DISABLE_COPY(GraphListener) /** \internal */ - void - __shutdown(bool); + void __shutdown(bool); rclcpp::Context::SharedPtr parent_context_; diff --git a/rclcpp/include/rclcpp/init_options.hpp b/rclcpp/include/rclcpp/init_options.hpp index fd0e4f82d7..693d936568 100644 --- a/rclcpp/include/rclcpp/init_options.hpp +++ b/rclcpp/include/rclcpp/init_options.hpp @@ -44,21 +44,17 @@ class InitOptions /// Assignment operator. RCLCPP_PUBLIC - InitOptions & - operator=(const InitOptions & other); + InitOptions & operator=(const InitOptions & other); RCLCPP_PUBLIC - virtual - ~InitOptions(); + virtual ~InitOptions(); /// Return the rcl init options. RCLCPP_PUBLIC - const rcl_init_options_t * - get_rcl_init_options() const; + const rcl_init_options_t * get_rcl_init_options() const; protected: - void - finalize_init_options(); + void finalize_init_options(); private: std::unique_ptr init_options_; diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/intra_process_manager.hpp index 5d3abed3f6..baff6c7596 100644 --- a/rclcpp/include/rclcpp/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/intra_process_manager.hpp @@ -24,14 +24,14 @@ #include #include #include +#include #include #include -#include #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/intra_process_manager_impl.hpp" -#include "rclcpp/mapped_ring_buffer.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/mapped_ring_buffer.hpp" #include "rclcpp/publisher_base.hpp" #include "rclcpp/subscription_base.hpp" #include "rclcpp/visibility_control.hpp" @@ -149,8 +149,7 @@ class IntraProcessManager * \return an unsigned 64-bit integer which is the subscription's unique id. */ RCLCPP_PUBLIC - uint64_t - add_subscription(SubscriptionBase::SharedPtr subscription); + uint64_t add_subscription(SubscriptionBase::SharedPtr subscription); /// Unregister a subscription using the subscription's unique id. /** @@ -159,8 +158,7 @@ class IntraProcessManager * \param intra_process_subscription_id id of the subscription to remove. */ RCLCPP_PUBLIC - void - remove_subscription(uint64_t intra_process_subscription_id); + void remove_subscription(uint64_t intra_process_subscription_id); /// Register a publisher with the manager, returns the publisher unique id. /** @@ -186,10 +184,7 @@ class IntraProcessManager * \return an unsigned 64-bit integer which is the publisher's unique id. */ RCLCPP_PUBLIC - uint64_t - add_publisher( - rclcpp::PublisherBase::SharedPtr publisher, - size_t buffer_size = 0); + uint64_t add_publisher(rclcpp::PublisherBase::SharedPtr publisher, size_t buffer_size = 0); /// Unregister a publisher using the publisher's unique id. /** @@ -198,8 +193,7 @@ class IntraProcessManager * \param intra_process_publisher_id id of the publisher to remove. */ RCLCPP_PUBLIC - void - remove_publisher(uint64_t intra_process_publisher_id); + void remove_publisher(uint64_t intra_process_publisher_id); /// Store a message in the manager, and return the message sequence number. /** @@ -232,18 +226,15 @@ class IntraProcessManager * \param message the message that is being stored. * \return the message sequence number. */ - template< - typename MessageT, typename Alloc = std::allocator> - uint64_t - store_intra_process_message( - uint64_t intra_process_publisher_id, - std::shared_ptr message) + template> + uint64_t store_intra_process_message( + uint64_t intra_process_publisher_id, std::shared_ptr message) { using MRBMessageAlloc = typename std::allocator_traits::template rebind_alloc; using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer; uint64_t message_seq = 0; - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->get_publisher_info_for_id( - intra_process_publisher_id, message_seq); + mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = + impl_->get_publisher_info_for_id(intra_process_publisher_id, message_seq); typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); if (!typed_buffer) { throw std::runtime_error("Typecast failed due to incorrect message type"); @@ -261,18 +252,17 @@ class IntraProcessManager } template< - typename MessageT, typename Alloc = std::allocator, + typename MessageT, + typename Alloc = std::allocator, typename Deleter = std::default_delete> - uint64_t - store_intra_process_message( - uint64_t intra_process_publisher_id, - std::unique_ptr message) + uint64_t store_intra_process_message( + uint64_t intra_process_publisher_id, std::unique_ptr message) { using MRBMessageAlloc = typename std::allocator_traits::template rebind_alloc; using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer; uint64_t message_seq = 0; - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->get_publisher_info_for_id( - intra_process_publisher_id, message_seq); + mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = + impl_->get_publisher_info_for_id(intra_process_publisher_id, message_seq); typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); if (!typed_buffer) { throw std::runtime_error("Typecast failed due to incorrect message type"); @@ -325,10 +315,10 @@ class IntraProcessManager * \param message the message typed unique_ptr used to return the message. */ template< - typename MessageT, typename Alloc = std::allocator, + typename MessageT, + typename Alloc = std::allocator, typename Deleter = std::default_delete> - void - take_intra_process_message( + void take_intra_process_message( uint64_t intra_process_publisher_id, uint64_t message_sequence_number, uint64_t requesting_subscriptions_intra_process_id, @@ -344,8 +334,7 @@ class IntraProcessManager intra_process_publisher_id, message_sequence_number, requesting_subscriptions_intra_process_id, - target_subs_size - ); + target_subs_size); typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); if (!typed_buffer) { return; @@ -360,10 +349,8 @@ class IntraProcessManager } } - template< - typename MessageT, typename Alloc = std::allocator> - void - take_intra_process_message( + template> + void take_intra_process_message( uint64_t intra_process_publisher_id, uint64_t message_sequence_number, uint64_t requesting_subscriptions_intra_process_id, @@ -379,8 +366,7 @@ class IntraProcessManager intra_process_publisher_id, message_sequence_number, requesting_subscriptions_intra_process_id, - target_subs_size - ); + target_subs_size); typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); if (!typed_buffer) { return; @@ -397,18 +383,15 @@ class IntraProcessManager /// Return true if the given rmw_gid_t matches any stored Publishers. RCLCPP_PUBLIC - bool - matches_any_publishers(const rmw_gid_t * id) const; + bool matches_any_publishers(const rmw_gid_t * id) const; /// Return the number of intraprocess subscriptions to a topic, given the publisher id. RCLCPP_PUBLIC - size_t - get_subscription_count(uint64_t intra_process_publisher_id) const; + size_t get_subscription_count(uint64_t intra_process_publisher_id) const; private: RCLCPP_PUBLIC - static uint64_t - get_next_unique_id(); + static uint64_t get_next_unique_id(); IntraProcessManagerImplBase::SharedPtr impl_; std::mutex take_mutex_; diff --git a/rclcpp/include/rclcpp/intra_process_manager_impl.hpp b/rclcpp/include/rclcpp/intra_process_manager_impl.hpp index ab29af7b92..30b5c3c367 100644 --- a/rclcpp/include/rclcpp/intra_process_manager_impl.hpp +++ b/rclcpp/include/rclcpp/intra_process_manager_impl.hpp @@ -51,11 +51,9 @@ class IntraProcessManagerImplBase IntraProcessManagerImplBase() = default; virtual ~IntraProcessManagerImplBase() = default; - virtual void - add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) = 0; + virtual void add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) = 0; - virtual void - remove_subscription(uint64_t intra_process_subscription_id) = 0; + virtual void remove_subscription(uint64_t intra_process_subscription_id) = 0; virtual void add_publisher( uint64_t id, @@ -63,29 +61,23 @@ class IntraProcessManagerImplBase mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb, size_t size) = 0; - virtual void - remove_publisher(uint64_t intra_process_publisher_id) = 0; + virtual void remove_publisher(uint64_t intra_process_publisher_id) = 0; - virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr - get_publisher_info_for_id( - uint64_t intra_process_publisher_id, - uint64_t & message_seq) = 0; + virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr get_publisher_info_for_id( + uint64_t intra_process_publisher_id, uint64_t & message_seq) = 0; - virtual void - store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0; + virtual void store_intra_process_message( + uint64_t intra_process_publisher_id, uint64_t message_seq) = 0; - virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr - take_intra_process_message( + virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr take_intra_process_message( uint64_t intra_process_publisher_id, uint64_t message_sequence_number, uint64_t requesting_subscriptions_intra_process_id, size_t & size) = 0; - virtual bool - matches_any_publishers(const rmw_gid_t * id) const = 0; + virtual bool matches_any_publishers(const rmw_gid_t * id) const = 0; - virtual size_t - get_subscription_count(uint64_t intra_process_publisher_id) const = 0; + virtual size_t get_subscription_count(uint64_t intra_process_publisher_id) const = 0; private: RCLCPP_DISABLE_COPY(IntraProcessManagerImplBase) @@ -98,15 +90,13 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase IntraProcessManagerImpl() = default; ~IntraProcessManagerImpl() = default; - void - add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) + void add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) { subscriptions_[id] = subscription; subscription_ids_by_topic_[fixed_size_string(subscription->get_topic_name())].insert(id); } - void - remove_subscription(uint64_t intra_process_subscription_id) + void remove_subscription(uint64_t intra_process_subscription_id) { subscriptions_.erase(intra_process_subscription_id); for (auto & pair : subscription_ids_by_topic_) { @@ -138,17 +128,14 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase publishers_[id].target_subscriptions_by_message_sequence.reserve(size); } - void - remove_publisher(uint64_t intra_process_publisher_id) + void remove_publisher(uint64_t intra_process_publisher_id) { publishers_.erase(intra_process_publisher_id); } // return message_seq and mrb - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - get_publisher_info_for_id( - uint64_t intra_process_publisher_id, - uint64_t & message_seq) + mapped_ring_buffer::MappedRingBufferBase::SharedPtr get_publisher_info_for_id( + uint64_t intra_process_publisher_id, uint64_t & message_seq) { std::lock_guard lock(runtime_mutex_); auto it = publishers_.find(intra_process_publisher_id); @@ -162,8 +149,7 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase return info.buffer; } - void - store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) + void store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) { std::lock_guard lock(runtime_mutex_); auto it = publishers_.find(intra_process_publisher_id); @@ -187,23 +173,20 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase info.target_subscriptions_by_message_sequence[message_seq].clear(); } std::copy( - destined_subscriptions.begin(), destined_subscriptions.end(), + destined_subscriptions.begin(), + destined_subscriptions.end(), // Memory allocation occurs in info.target_subscriptions_by_message_sequence[message_seq] std::inserter( info.target_subscriptions_by_message_sequence[message_seq], // This ends up only being a hint to std::set, could also be .begin(). - info.target_subscriptions_by_message_sequence[message_seq].end() - ) - ); + info.target_subscriptions_by_message_sequence[message_seq].end())); } - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - take_intra_process_message( + mapped_ring_buffer::MappedRingBufferBase::SharedPtr take_intra_process_message( uint64_t intra_process_publisher_id, uint64_t message_sequence_number, uint64_t requesting_subscriptions_intra_process_id, - size_t & size - ) + size_t & size) { std::lock_guard lock(runtime_mutex_); PublisherInfo * info; @@ -227,8 +210,7 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase } { auto it = std::find( - target_subs->begin(), target_subs->end(), - requesting_subscriptions_intra_process_id); + target_subs->begin(), target_subs->end(), requesting_subscriptions_intra_process_id); if (it == target_subs->end()) { // This publisher id/message seq pair was not intended for this subscription. return 0; @@ -239,8 +221,7 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase return info->buffer; } - bool - matches_any_publishers(const rmw_gid_t * id) const + bool matches_any_publishers(const rmw_gid_t * id) const { for (auto & publisher_pair : publishers_) { auto publisher = publisher_pair.second.publisher.lock(); @@ -254,8 +235,7 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase return false; } - size_t - get_subscription_count(uint64_t intra_process_publisher_id) const + size_t get_subscription_count(uint64_t intra_process_publisher_id) const { auto publisher_it = publishers_.find(intra_process_publisher_id); if (publisher_it == publishers_.end()) { @@ -280,8 +260,7 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase using FixedSizeString = std::array; - FixedSizeString - fixed_size_string(const char * str) const + FixedSizeString fixed_size_string(const char * str) const { FixedSizeString ret; size_t size = std::strlen(str) + 1; @@ -293,8 +272,7 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase } struct strcmp_wrapper { - bool - operator()(const FixedSizeString lhs, const FixedSizeString rhs) const + bool operator()(const FixedSizeString lhs, const FixedSizeString rhs) const { return std::strcmp(lhs.data(), rhs.data()) < 0; } @@ -307,8 +285,10 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase using AllocSet = std::set, RebindAlloc>; using SubscriptionMap = std::unordered_map< - uint64_t, SubscriptionBase::WeakPtr, - std::hash, std::equal_to, + uint64_t, + SubscriptionBase::WeakPtr, + std::hash, + std::equal_to, RebindAlloc>>; using IDTopicMap = std::map< @@ -332,15 +312,19 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer; using TargetSubscriptionsMap = std::unordered_map< - uint64_t, AllocSet, - std::hash, std::equal_to, + uint64_t, + AllocSet, + std::hash, + std::equal_to, RebindAlloc>>; TargetSubscriptionsMap target_subscriptions_by_message_sequence; }; using PublisherMap = std::unordered_map< - uint64_t, PublisherInfo, - std::hash, std::equal_to, + uint64_t, + PublisherInfo, + std::hash, + std::equal_to, RebindAlloc>>; PublisherMap publishers_; @@ -349,8 +333,7 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase }; RCLCPP_PUBLIC -IntraProcessManagerImplBase::SharedPtr -create_default_impl(); +IntraProcessManagerImplBase::SharedPtr create_default_impl(); } // namespace intra_process_manager } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/intra_process_setting.hpp b/rclcpp/include/rclcpp/intra_process_setting.hpp index 8e4b44eb64..9c8f10c803 100644 --- a/rclcpp/include/rclcpp/intra_process_setting.hpp +++ b/rclcpp/include/rclcpp/intra_process_setting.hpp @@ -19,8 +19,7 @@ namespace rclcpp { /// Used as argument in create_publisher and create_subscriber. -enum class IntraProcessSetting -{ +enum class IntraProcessSetting { /// Explicitly enable intraprocess comm at publisher/subscription level. Enable, /// Explicitly disable intraprocess comm at publisher/subscription level. diff --git a/rclcpp/include/rclcpp/logger.hpp b/rclcpp/include/rclcpp/logger.hpp index 7e7049dc6b..c9aba58c62 100644 --- a/rclcpp/include/rclcpp/logger.hpp +++ b/rclcpp/include/rclcpp/logger.hpp @@ -59,8 +59,7 @@ class Logger; * \return a dummy logger if logging is disabled. */ RCLCPP_PUBLIC -Logger -get_logger(const std::string & name); +Logger get_logger(const std::string & name); /// Return a named logger using an rcl_node_t. /** @@ -71,8 +70,7 @@ get_logger(const std::string & name); * \return a logger based on the node name, or "rclcpp" if there's an error */ RCLCPP_PUBLIC -Logger -get_node_logger(const rcl_node_t * node); +Logger get_node_logger(const rcl_node_t * node); class Logger { @@ -85,15 +83,17 @@ class Logger * This is used when logging is disabled: see `RCLCPP_LOGGING_ENABLED`. * This cannot be called directly, see `rclcpp::get_logger` instead. */ - Logger() - : name_(nullptr) {} + Logger() : name_(nullptr) + { + } /// Constructor of a named logger. /** * This cannot be called directly, see `rclcpp::get_logger` instead. */ - explicit Logger(const std::string & name) - : name_(new std::string(name)) {} + explicit Logger(const std::string & name) : name_(new std::string(name)) + { + } std::shared_ptr name_; @@ -108,8 +108,7 @@ class Logger * disabled). */ RCLCPP_PUBLIC - const char * - get_name() const + const char * get_name() const { if (!name_) { return nullptr; @@ -130,8 +129,7 @@ class Logger * disabled). */ RCLCPP_PUBLIC - Logger - get_child(const std::string & suffix) + Logger get_child(const std::string & suffix) { if (!name_) { return Logger(); diff --git a/rclcpp/include/rclcpp/macros.hpp b/rclcpp/include/rclcpp/macros.hpp index f224abe60a..26a602e88b 100644 --- a/rclcpp/include/rclcpp/macros.hpp +++ b/rclcpp/include/rclcpp/macros.hpp @@ -23,7 +23,7 @@ * * Use in the private section of the class. */ -#define RCLCPP_DISABLE_COPY(...) \ +#define RCLCPP_DISABLE_COPY(...) \ __VA_ARGS__(const __VA_ARGS__ &) = delete; \ __VA_ARGS__ & operator=(const __VA_ARGS__ &) = delete; @@ -33,9 +33,9 @@ * Use in the public section of the class. * Make sure to include `` in the header when using this. */ -#define RCLCPP_SMART_PTR_DEFINITIONS(...) \ +#define RCLCPP_SMART_PTR_DEFINITIONS(...) \ RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \ - RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \ + RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \ RCLCPP_UNIQUE_PTR_DEFINITIONS(__VA_ARGS__) /** @@ -49,8 +49,8 @@ * Make sure to include `` in the header when using this. */ #define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...) \ - RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \ - RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \ + RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \ + RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \ __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) /** @@ -64,28 +64,27 @@ * Make sure to include `` in the header when using this. */ #define RCLCPP_SMART_PTR_ALIASES_ONLY(...) \ - __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) \ __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__) -#define __RCLCPP_SHARED_PTR_ALIAS(...) \ +#define __RCLCPP_SHARED_PTR_ALIAS(...) \ using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ using ConstSharedPtr = std::shared_ptr; -#define __RCLCPP_MAKE_SHARED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ +#define __RCLCPP_MAKE_SHARED_DEFINITION(...) \ + template \ + static std::shared_ptr<__VA_ARGS__> make_shared(Args &&... args) \ + { \ + return std::make_shared<__VA_ARGS__>(std::forward(args)...); \ } /// Defines aliases and static functions for using the Class with shared_ptrs. #define RCLCPP_SHARED_PTR_DEFINITIONS(...) \ - __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \ __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__) -#define __RCLCPP_WEAK_PTR_ALIAS(...) \ +#define __RCLCPP_WEAK_PTR_ALIAS(...) \ using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ using ConstWeakPtr = std::weak_ptr; @@ -94,20 +93,19 @@ #define __RCLCPP_UNIQUE_PTR_ALIAS(...) using UniquePtr = std::unique_ptr<__VA_ARGS__>; -#define __RCLCPP_MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ +#define __RCLCPP_MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> make_unique(Args &&... args) \ + { \ + return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args)...)); \ } /// Defines aliases and static functions for using the Class with unique_ptrs. #define RCLCPP_UNIQUE_PTR_DEFINITIONS(...) \ - __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ __RCLCPP_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) #define RCLCPP_STRING_JOIN(arg1, arg2) RCLCPP_DO_STRING_JOIN(arg1, arg2) -#define RCLCPP_DO_STRING_JOIN(arg1, arg2) arg1 ## arg2 +#define RCLCPP_DO_STRING_JOIN(arg1, arg2) arg1##arg2 #endif // RCLCPP__MACROS_HPP_ diff --git a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp index c8fdf6493d..ae922ae5b4 100644 --- a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp +++ b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp @@ -88,7 +88,9 @@ class MappedRingBuffer : public MappedRingBufferBase } } - virtual ~MappedRingBuffer() {} + virtual ~MappedRingBuffer() + { + } /// Return a copy of the value stored in the ring buffer at the given key. /** @@ -102,8 +104,7 @@ class MappedRingBuffer : public MappedRingBufferBase * \param key the key associated with the stored value * \param value if the key is found, the value is stored in this parameter */ - void - get(uint64_t key, ElemUniquePtr & value) + void get(uint64_t key, ElemUniquePtr & value) { std::lock_guard lock(data_mutex_); auto it = get_iterator_of_key(key); @@ -140,8 +141,7 @@ class MappedRingBuffer : public MappedRingBufferBase * \param key the key associated with the stored value * \param value if the key is found, the value is stored in this parameter */ - void - get(uint64_t key, ConstElemSharedPtr & value) + void get(uint64_t key, ConstElemSharedPtr & value) { std::lock_guard lock(data_mutex_); auto it = get_iterator_of_key(key); @@ -175,8 +175,7 @@ class MappedRingBuffer : public MappedRingBufferBase * \param key the key associated with the stored value * \param value if the key is found, the value is stored in this parameter */ - void - pop(uint64_t key, ElemUniquePtr & value) + void pop(uint64_t key, ElemUniquePtr & value) { std::lock_guard lock(data_mutex_); auto it = get_iterator_of_key(key); @@ -212,8 +211,7 @@ class MappedRingBuffer : public MappedRingBufferBase * \param key the key associated with the stored value * \param value if the key is found, the value is stored in this parameter */ - void - pop(uint64_t key, ConstElemSharedPtr & value) + void pop(uint64_t key, ConstElemSharedPtr & value) { std::lock_guard lock(data_mutex_); auto it = get_iterator_of_key(key); @@ -241,8 +239,7 @@ class MappedRingBuffer : public MappedRingBufferBase * \param key the key associated with the value to be stored * \param value the value to store, and optionally the value displaced */ - bool - push_and_replace(uint64_t key, ConstElemSharedPtr value) + bool push_and_replace(uint64_t key, ConstElemSharedPtr value) { std::lock_guard lock(data_mutex_); bool did_replace = elements_[head_].in_use; @@ -260,8 +257,7 @@ class MappedRingBuffer : public MappedRingBufferBase /** * See `bool push_and_replace(uint64_t key, const ConstElemSharedPtr & value)`. */ - bool - push_and_replace(uint64_t key, ElemUniquePtr value) + bool push_and_replace(uint64_t key, ElemUniquePtr value) { std::lock_guard lock(data_mutex_); bool did_replace = elements_[head_].in_use; @@ -276,8 +272,7 @@ class MappedRingBuffer : public MappedRingBufferBase } /// Return true if the key is found in the ring buffer, otherwise false. - bool - has_key(uint64_t key) + bool has_key(uint64_t key) { std::lock_guard lock(data_mutex_); return elements_.end() != get_iterator_of_key(key); @@ -296,14 +291,11 @@ class MappedRingBuffer : public MappedRingBufferBase using VectorAlloc = typename std::allocator_traits::template rebind_alloc; - typename std::vector::iterator - get_iterator_of_key(uint64_t key) + typename std::vector::iterator get_iterator_of_key(uint64_t key) { - auto it = std::find_if( - elements_.begin(), elements_.end(), - [key](Element & e) -> bool { - return e.key == key && e.in_use; - }); + auto it = std::find_if(elements_.begin(), elements_.end(), [key](Element & e) -> bool { + return e.key == key && e.in_use; + }); return it; } diff --git a/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp index a3950d9bd0..9135c7fb9f 100644 --- a/rclcpp/include/rclcpp/memory_strategies.hpp +++ b/rclcpp/include/rclcpp/memory_strategies.hpp @@ -24,8 +24,7 @@ namespace memory_strategies { RCLCPP_PUBLIC -memory_strategy::MemoryStrategy::SharedPtr -create_default_strategy(); +memory_strategy::MemoryStrategy::SharedPtr create_default_strategy(); } // namespace memory_strategies } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index aaf730b9b2..c8bdc4ad50 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -64,83 +64,52 @@ class RCLCPP_PUBLIC MemoryStrategy virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; - virtual void - get_next_subscription( - rclcpp::executor::AnyExecutable & any_exec, - const WeakNodeList & weak_nodes) = 0; - - virtual void - get_next_service( - rclcpp::executor::AnyExecutable & any_exec, - const WeakNodeList & weak_nodes) = 0; - - virtual void - get_next_client( - rclcpp::executor::AnyExecutable & any_exec, - const WeakNodeList & weak_nodes) = 0; - - virtual void - get_next_timer( - rclcpp::executor::AnyExecutable & any_exec, - const WeakNodeList & weak_nodes) = 0; - - virtual void - get_next_waitable( - rclcpp::executor::AnyExecutable & any_exec, - const WeakNodeList & weak_nodes) = 0; - - virtual rcl_allocator_t - get_allocator() = 0; - - static rclcpp::SubscriptionBase::SharedPtr - get_subscription_by_handle( - std::shared_ptr subscriber_handle, - const WeakNodeList & weak_nodes); - - static rclcpp::ServiceBase::SharedPtr - get_service_by_handle( - std::shared_ptr service_handle, - const WeakNodeList & weak_nodes); - - static rclcpp::ClientBase::SharedPtr - get_client_by_handle( - std::shared_ptr client_handle, - const WeakNodeList & weak_nodes); - - static rclcpp::TimerBase::SharedPtr - get_timer_by_handle( - std::shared_ptr timer_handle, - const WeakNodeList & weak_nodes); - - static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_by_group( - rclcpp::callback_group::CallbackGroup::SharedPtr group, - const WeakNodeList & weak_nodes); - - static rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription, - const WeakNodeList & weak_nodes); - - static rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_service( - rclcpp::ServiceBase::SharedPtr service, - const WeakNodeList & weak_nodes); - - static rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_client( - rclcpp::ClientBase::SharedPtr client, - const WeakNodeList & weak_nodes); - - static rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_timer( - rclcpp::TimerBase::SharedPtr timer, - const WeakNodeList & weak_nodes); - - static rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_waitable( - rclcpp::Waitable::SharedPtr waitable, - const WeakNodeList & weak_nodes); + virtual void get_next_subscription( + rclcpp::executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) = 0; + + virtual void get_next_service( + rclcpp::executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) = 0; + + virtual void get_next_client( + rclcpp::executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) = 0; + + virtual void get_next_timer( + rclcpp::executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) = 0; + + virtual void get_next_waitable( + rclcpp::executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) = 0; + + virtual rcl_allocator_t get_allocator() = 0; + + static rclcpp::SubscriptionBase::SharedPtr get_subscription_by_handle( + std::shared_ptr subscriber_handle, const WeakNodeList & weak_nodes); + + static rclcpp::ServiceBase::SharedPtr get_service_by_handle( + std::shared_ptr service_handle, const WeakNodeList & weak_nodes); + + static rclcpp::ClientBase::SharedPtr get_client_by_handle( + std::shared_ptr client_handle, const WeakNodeList & weak_nodes); + + static rclcpp::TimerBase::SharedPtr get_timer_by_handle( + std::shared_ptr timer_handle, const WeakNodeList & weak_nodes); + + static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group( + rclcpp::callback_group::CallbackGroup::SharedPtr group, const WeakNodeList & weak_nodes); + + static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription, const WeakNodeList & weak_nodes); + + static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_service( + rclcpp::ServiceBase::SharedPtr service, const WeakNodeList & weak_nodes); + + static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_client( + rclcpp::ClientBase::SharedPtr client, const WeakNodeList & weak_nodes); + + static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_timer( + rclcpp::TimerBase::SharedPtr timer, const WeakNodeList & weak_nodes); + + static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_waitable( + rclcpp::Waitable::SharedPtr waitable, const WeakNodeList & weak_nodes); }; } // namespace memory_strategy diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index 14a9f74128..995e3f8bbd 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -95,15 +95,13 @@ class MessageMemoryStrategy rclcpp::exceptions::throw_from_rcl_error(ret); } - auto serialized_msg = std::shared_ptr( - msg, - [](rmw_serialized_message_t * msg) { + auto serialized_msg = + std::shared_ptr(msg, [](rmw_serialized_message_t * msg) { auto ret = rmw_serialized_message_fini(msg); delete msg; if (ret != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy serialized message: %s", rcl_get_error_string().str); + "rclcpp", "failed to destroy serialized message: %s", rcl_get_error_string().str); } }); diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index e46d2626ba..0076bdb06b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -80,9 +80,7 @@ class Node : public std::enable_shared_from_this * \param[in] options Additional options to control creation of the node. */ RCLCPP_PUBLIC - explicit Node( - const std::string & node_name, - const NodeOptions & options = NodeOptions()); + explicit Node(const std::string & node_name, const NodeOptions & options = NodeOptions()); /// Create a new node with the specified name. /** @@ -102,8 +100,7 @@ class Node : public std::enable_shared_from_this /// Get the name of the node. /** \return The name of the node. */ RCLCPP_PUBLIC - const char * - get_name() const; + const char * get_name() const; /// Get the namespace of the node. /** @@ -116,32 +113,28 @@ class Node : public std::enable_shared_from_this * \return The namespace of the node. */ RCLCPP_PUBLIC - const char * - get_namespace() const; + const char * get_namespace() const; /// Get the fully-qualified name of the node. /** * The fully-qualified name includes the local namespace and name of the node. */ RCLCPP_PUBLIC - const char * - get_fully_qualified_name() const; + const char * get_fully_qualified_name() const; /// Get the logger of the node. /** \return The logger of the node. */ RCLCPP_PUBLIC - rclcpp::Logger - get_logger() const; + rclcpp::Logger get_logger() const; /// Create and return a callback group. RCLCPP_PUBLIC - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + rclcpp::callback_group::CallbackGroup::SharedPtr create_callback_group( + rclcpp::callback_group::CallbackGroupType group_type); /// Return the list of callback groups in the node. RCLCPP_PUBLIC - const std::vector & - get_callback_groups() const; + const std::vector & get_callback_groups() const; /// Create and return a Publisher. /** @@ -175,13 +168,11 @@ class Node : public std::enable_shared_from_this typename MessageT, typename AllocatorT = std::allocator, typename PublisherT = rclcpp::Publisher> - std::shared_ptr - create_publisher( + std::shared_ptr create_publisher( const std::string & topic_name, const rclcpp::QoS & qos, const PublisherOptionsWithAllocator & options = - PublisherOptionsWithAllocator() - ); + PublisherOptionsWithAllocator()); /// Create and return a Subscription. /** @@ -201,18 +192,17 @@ class Node : public std::enable_shared_from_this typename CallbackT, typename AllocatorT = std::allocator, typename SubscriptionT = rclcpp::Subscription< - typename rclcpp::subscription_traits::has_message_type::type, AllocatorT>> - std::shared_ptr - create_subscription( + typename rclcpp::subscription_traits::has_message_type::type, + AllocatorT>> + std::shared_ptr create_subscription( const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, const SubscriptionOptionsWithAllocator & options = - SubscriptionOptionsWithAllocator(), + SubscriptionOptionsWithAllocator(), typename rclcpp::message_memory_strategy::MessageMemoryStrategy< - typename rclcpp::subscription_traits::has_message_type::type, AllocatorT - >::SharedPtr - msg_mem_strat = nullptr); + typename rclcpp::subscription_traits::has_message_type::type, + AllocatorT>::SharedPtr msg_mem_strat = nullptr); /// Create a timer. /** @@ -221,24 +211,21 @@ class Node : public std::enable_shared_from_this * \param[in] group Callback group to execute this timer's callback in. */ template - typename rclcpp::WallTimer::SharedPtr - create_wall_timer( + typename rclcpp::WallTimer::SharedPtr create_wall_timer( std::chrono::duration period, CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Client. */ template - typename rclcpp::Client::SharedPtr - create_client( + typename rclcpp::Client::SharedPtr create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Service. */ template - typename rclcpp::Service::SharedPtr - create_service( + typename rclcpp::Service::SharedPtr create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, @@ -285,12 +272,11 @@ class Node : public std::enable_shared_from_this * value fails to be set. */ RCLCPP_PUBLIC - const rclcpp::ParameterValue & - declare_parameter( + const rclcpp::ParameterValue & declare_parameter( const std::string & name, const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(), const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor(), + rcl_interfaces::msg::ParameterDescriptor(), bool ignore_override = false); /// Declare and initialize a parameter with a type. @@ -315,12 +301,11 @@ class Node : public std::enable_shared_from_this * - https://www.youtube.com/watch?v=uQyT-5iWUow (cppnow 2018 presentation) */ template - auto - declare_parameter( + auto declare_parameter( const std::string & name, const ParameterT & default_value, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor(), + rcl_interfaces::msg::ParameterDescriptor(), bool ignore_override = false); /// Declare and initialize several parameters with the same namespace and type. @@ -360,8 +345,7 @@ class Node : public std::enable_shared_from_this * value fails to be set. */ template - std::vector - declare_parameters( + std::vector declare_parameters( const std::string & namespace_, const std::map & parameters, bool ignore_overrides = false); @@ -374,13 +358,10 @@ class Node : public std::enable_shared_from_this * See the simpler declare_parameters() on this class for more details. */ template - std::vector - declare_parameters( + std::vector declare_parameters( const std::string & namespace_, - const std::map< - std::string, - std::pair - > & parameters, + const std::map> & + parameters, bool ignore_overrides = false); /// Undeclare a previously declared parameter. @@ -395,8 +376,7 @@ class Node : public std::enable_shared_from_this * was create as read_only (immutable). */ RCLCPP_PUBLIC - void - undeclare_parameter(const std::string & name); + void undeclare_parameter(const std::string & name); /// Return true if a given parameter is declared. /** @@ -404,8 +384,7 @@ class Node : public std::enable_shared_from_this * \return true if the parameter name has been declared, otherwise false. */ RCLCPP_PUBLIC - bool - has_parameter(const std::string & name) const; + bool has_parameter(const std::string & name) const; /// Set a single parameter. /** @@ -437,8 +416,7 @@ class Node : public std::enable_shared_from_this * has not been declared and undeclared parameters are not allowed. */ RCLCPP_PUBLIC - rcl_interfaces::msg::SetParametersResult - set_parameter(const rclcpp::Parameter & parameter); + rcl_interfaces::msg::SetParametersResult set_parameter(const rclcpp::Parameter & parameter); /// Set one or more parameters, one at a time. /** @@ -475,8 +453,8 @@ class Node : public std::enable_shared_from_this * has not been declared and undeclared parameters are not allowed. */ RCLCPP_PUBLIC - std::vector - set_parameters(const std::vector & parameters); + std::vector set_parameters( + const std::vector & parameters); /// Set one or more parameters, all at once. /** @@ -509,8 +487,8 @@ class Node : public std::enable_shared_from_this * has not been declared and undeclared parameters are not allowed. */ RCLCPP_PUBLIC - rcl_interfaces::msg::SetParametersResult - set_parameters_atomically(const std::vector & parameters); + rcl_interfaces::msg::SetParametersResult set_parameters_atomically( + const std::vector & parameters); /// Return the parameter by the given name. /** @@ -529,8 +507,7 @@ class Node : public std::enable_shared_from_this * has not been declared and undeclared parameters are not allowed. */ RCLCPP_PUBLIC - rclcpp::Parameter - get_parameter(const std::string & name) const; + rclcpp::Parameter get_parameter(const std::string & name) const; /// Get the value of a parameter by the given name, and return true if it was set. /** @@ -548,8 +525,7 @@ class Node : public std::enable_shared_from_this * \return true if the parameter was previously declared, otherwise false. */ RCLCPP_PUBLIC - bool - get_parameter(const std::string & name, rclcpp::Parameter & parameter) const; + bool get_parameter(const std::string & name, rclcpp::Parameter & parameter) const; /// Get the value of a parameter by the given name, and return true if it was set. /** @@ -565,8 +541,7 @@ class Node : public std::enable_shared_from_this * match the value of the parameter which is stored. */ template - bool - get_parameter(const std::string & name, ParameterT & parameter) const; + bool get_parameter(const std::string & name, ParameterT & parameter) const; /// Get the parameter value, or the "alternative_value" if not set, and assign it to "parameter". /** @@ -584,11 +559,8 @@ class Node : public std::enable_shared_from_this * \returns true if the parameter was set, false otherwise. */ template - bool - get_parameter_or( - const std::string & name, - ParameterT & parameter, - const ParameterT & alternative_value) const; + bool get_parameter_or( + const std::string & name, ParameterT & parameter, const ParameterT & alternative_value) const; /// Return the parameters by the given parameter names. /** @@ -609,8 +581,7 @@ class Node : public std::enable_shared_from_this * allowed. */ RCLCPP_PUBLIC - std::vector - get_parameters(const std::vector & names) const; + std::vector get_parameters(const std::vector & names) const; /// Get the parameter values for all parameters that have a given prefix. /** @@ -650,10 +621,7 @@ class Node : public std::enable_shared_from_this * match the value of the parameter which is stored. */ template - bool - get_parameters( - const std::string & prefix, - std::map & values) const; + bool get_parameters(const std::string & prefix, std::map & values) const; /// Return the parameter descriptor for the given parameter name. /** @@ -672,8 +640,7 @@ class Node : public std::enable_shared_from_this * allowed. */ RCLCPP_PUBLIC - rcl_interfaces::msg::ParameterDescriptor - describe_parameter(const std::string & name) const; + rcl_interfaces::msg::ParameterDescriptor describe_parameter(const std::string & name) const; /// Return a vector of parameter descriptors, one for each of the given names. /** @@ -694,8 +661,8 @@ class Node : public std::enable_shared_from_this * allowed. */ RCLCPP_PUBLIC - std::vector - describe_parameters(const std::vector & names) const; + std::vector describe_parameters( + const std::vector & names) const; /// Return a vector of parameter types, one for each of the given names. /** @@ -714,19 +681,17 @@ class Node : public std::enable_shared_from_this * allowed. */ RCLCPP_PUBLIC - std::vector - get_parameter_types(const std::vector & names) const; + std::vector get_parameter_types(const std::vector & names) const; /// Return a list of parameters with any of the given prefixes, up to the given depth. /** * \todo: properly document and test this method. */ RCLCPP_PUBLIC - rcl_interfaces::msg::ListParametersResult - list_parameters(const std::vector & prefixes, uint64_t depth) const; + rcl_interfaces::msg::ListParametersResult list_parameters( + const std::vector & prefixes, uint64_t depth) const; - using OnSetParametersCallbackHandle = - rclcpp::node_interfaces::OnSetParametersCallbackHandle; + using OnSetParametersCallbackHandle = rclcpp::node_interfaces::OnSetParametersCallbackHandle; using OnParametersSetCallbackType = rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; @@ -792,8 +757,8 @@ class Node : public std::enable_shared_from_this * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails. */ RCLCPP_PUBLIC - OnSetParametersCallbackHandle::SharedPtr - add_on_set_parameters_callback(OnParametersSetCallbackType callback); + OnSetParametersCallbackHandle::SharedPtr add_on_set_parameters_callback( + OnParametersSetCallbackType callback); /// Remove a callback registered with `add_on_set_parameters_callback`. /** @@ -819,8 +784,7 @@ class Node : public std::enable_shared_from_this * or if it has been removed before. */ RCLCPP_PUBLIC - void - remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler); + void remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler); /// Register a callback to be called anytime a parameter is about to be changed. /** @@ -837,8 +801,8 @@ class Node : public std::enable_shared_from_this * otherwise nullptr. */ RCLCPP_PUBLIC - OnParametersSetCallbackType - set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback); + OnParametersSetCallbackType set_on_parameters_set_callback( + rclcpp::Node::OnParametersSetCallbackType callback); /// Get the fully-qualified names of all available nodes. /** @@ -846,24 +810,19 @@ class Node : public std::enable_shared_from_this * \return A vector of fully-qualified names of nodes. */ RCLCPP_PUBLIC - std::vector - get_node_names() const; + std::vector get_node_names() const; RCLCPP_PUBLIC - std::map> - get_topic_names_and_types() const; + std::map> get_topic_names_and_types() const; RCLCPP_PUBLIC - std::map> - get_service_names_and_types() const; + std::map> get_service_names_and_types() const; RCLCPP_PUBLIC - size_t - count_publishers(const std::string & topic_name) const; + size_t count_publishers(const std::string & topic_name) const; RCLCPP_PUBLIC - size_t - count_subscribers(const std::string & topic_name) const; + size_t count_subscribers(const std::string & topic_name) const; /// Return a graph event, which will be set anytime a graph change occurs. /* The graph Event object is a loan which must be returned. @@ -871,8 +830,7 @@ class Node : public std::enable_shared_from_this * out of scope. */ RCLCPP_PUBLIC - rclcpp::Event::SharedPtr - get_graph_event(); + rclcpp::Event::SharedPtr get_graph_event(); /// Wait for a graph event to occur by waiting on an Event to become set. /** @@ -883,68 +841,53 @@ class Node : public std::enable_shared_from_this * get_graph_event(). */ RCLCPP_PUBLIC - void - wait_for_graph_change( - rclcpp::Event::SharedPtr event, - std::chrono::nanoseconds timeout); + void wait_for_graph_change(rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout); RCLCPP_PUBLIC - rclcpp::Clock::SharedPtr - get_clock(); + rclcpp::Clock::SharedPtr get_clock(); RCLCPP_PUBLIC - Time - now(); + Time now(); /// Return the Node's internal NodeBaseInterface implementation. RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_base_interface(); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); /// Return the Node's internal NodeClockInterface implementation. RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeClockInterface::SharedPtr - get_node_clock_interface(); + rclcpp::node_interfaces::NodeClockInterface::SharedPtr get_node_clock_interface(); /// Return the Node's internal NodeGraphInterface implementation. RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeGraphInterface::SharedPtr - get_node_graph_interface(); + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr get_node_graph_interface(); /// Return the Node's internal NodeLoggingInterface implementation. RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr - get_node_logging_interface(); + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr get_node_logging_interface(); /// Return the Node's internal NodeTimersInterface implementation. RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeTimersInterface::SharedPtr - get_node_timers_interface(); + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr get_node_timers_interface(); /// Return the Node's internal NodeTopicsInterface implementation. RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr - get_node_topics_interface(); + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr get_node_topics_interface(); /// Return the Node's internal NodeServicesInterface implementation. RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr - get_node_services_interface(); + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr get_node_services_interface(); /// Return the Node's internal NodeWaitablesInterface implementation. RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr - get_node_waitables_interface(); + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr get_node_waitables_interface(); /// Return the Node's internal NodeParametersInterface implementation. RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr - get_node_parameters_interface(); + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface(); /// Return the Node's internal NodeParametersInterface implementation. RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr - get_node_time_source_interface(); + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface(); /// Return the sub-namespace, if this is a sub-node, otherwise an empty string. /** @@ -975,8 +918,7 @@ class Node : public std::enable_shared_from_this * \return the sub-namespace string, not including the node's original namespace */ RCLCPP_PUBLIC - const std::string & - get_sub_namespace() const; + const std::string & get_sub_namespace() const; /// Return the effective namespace that is used when creating entities. /** @@ -1003,8 +945,7 @@ class Node : public std::enable_shared_from_this * \return the sub-namespace string, not including the node's original namespace */ RCLCPP_PUBLIC - const std::string & - get_effective_namespace() const; + const std::string & get_effective_namespace() const; /// Create a sub-node, which will extend the namespace of all entities created with it. /** @@ -1045,13 +986,11 @@ class Node : public std::enable_shared_from_this * with a leading '/'. */ RCLCPP_PUBLIC - rclcpp::Node::SharedPtr - create_sub_node(const std::string & sub_namespace); + rclcpp::Node::SharedPtr create_sub_node(const std::string & sub_namespace); /// Return the NodeOptions used when creating this node. RCLCPP_PUBLIC - const rclcpp::NodeOptions & - get_node_options() const; + const rclcpp::NodeOptions & get_node_options() const; /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE). /** @@ -1063,8 +1002,7 @@ class Node : public std::enable_shared_from_this */ RCLCPP_PUBLIC RCUTILS_WARN_UNUSED - bool - assert_liveliness() const; + bool assert_liveliness() const; protected: /// Construct a sub-node, which will extend the namespace of all entities created with it. @@ -1075,16 +1013,13 @@ class Node : public std::enable_shared_from_this * \param[in] sub_namespace The sub-namespace of the sub-node. */ RCLCPP_PUBLIC - Node( - const Node & other, - const std::string & sub_namespace); + Node(const Node & other, const std::string & sub_namespace); private: RCLCPP_DISABLE_COPY(Node) RCLCPP_PUBLIC - bool - group_in_node(callback_group::CallbackGroup::SharedPtr group); + bool group_in_node(callback_group::CallbackGroup::SharedPtr group); rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index e7c30d596c..8fe61c8315 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -54,9 +54,8 @@ namespace rclcpp { RCLCPP_LOCAL -inline -std::string -extend_name_with_sub_namespace(const std::string & name, const std::string & sub_namespace) +inline std::string extend_name_with_sub_namespace( + const std::string & name, const std::string & sub_namespace) { std::string name_with_sub_namespace(name); if (sub_namespace != "" && name.front() != '/' && name.front() != '~') { @@ -66,33 +65,24 @@ extend_name_with_sub_namespace(const std::string & name, const std::string & sub } template -std::shared_ptr -Node::create_publisher( +std::shared_ptr Node::create_publisher( const std::string & topic_name, const rclcpp::QoS & qos, const PublisherOptionsWithAllocator & options) { return rclcpp::create_publisher( - *this, - extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), - qos, - options); + *this, extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), qos, options); } -template< - typename MessageT, - typename CallbackT, - typename AllocatorT, - typename SubscriptionT> -std::shared_ptr -Node::create_subscription( +template +std::shared_ptr Node::create_subscription( const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, const SubscriptionOptionsWithAllocator & options, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< - typename rclcpp::subscription_traits::has_message_type::type, AllocatorT>::SharedPtr - msg_mem_strat) + typename rclcpp::subscription_traits::has_message_type::type, + AllocatorT>::SharedPtr msg_mem_strat) { return rclcpp::create_subscription( *this, @@ -104,8 +94,7 @@ Node::create_subscription( } template -typename rclcpp::WallTimer::SharedPtr -Node::create_wall_timer( +typename rclcpp::WallTimer::SharedPtr Node::create_wall_timer( std::chrono::duration period, CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) @@ -119,8 +108,7 @@ Node::create_wall_timer( } template -typename Client::SharedPtr -Node::create_client( +typename Client::SharedPtr Node::create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile, rclcpp::callback_group::CallbackGroup::SharedPtr group) @@ -135,8 +123,7 @@ Node::create_client( } template -typename rclcpp::Service::SharedPtr -Node::create_service( +typename rclcpp::Service::SharedPtr Node::create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, @@ -152,24 +139,20 @@ Node::create_service( } template -auto -Node::declare_parameter( +auto Node::declare_parameter( const std::string & name, const ParameterT & default_value, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, bool ignore_override) { - return this->declare_parameter( - name, - rclcpp::ParameterValue(default_value), - parameter_descriptor, - ignore_override - ).get(); + return this + ->declare_parameter( + name, rclcpp::ParameterValue(default_value), parameter_descriptor, ignore_override) + .get(); } template -std::vector -Node::declare_parameters( +std::vector Node::declare_parameters( const std::string & namespace_, const std::map & parameters, bool ignore_overrides) @@ -177,48 +160,44 @@ Node::declare_parameters( std::vector result; std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); std::transform( - parameters.begin(), parameters.end(), std::back_inserter(result), + parameters.begin(), + parameters.end(), + std::back_inserter(result), [this, &normalized_namespace, ignore_overrides](auto element) { return this->declare_parameter( normalized_namespace + element.first, element.second, rcl_interfaces::msg::ParameterDescriptor(), ignore_overrides); - } - ); + }); return result; } template -std::vector -Node::declare_parameters( +std::vector Node::declare_parameters( const std::string & namespace_, - const std::map< - std::string, - std::pair - > & parameters, + const std::map > & + parameters, bool ignore_overrides) { std::vector result; std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); std::transform( - parameters.begin(), parameters.end(), std::back_inserter(result), + parameters.begin(), + parameters.end(), + std::back_inserter(result), [this, &normalized_namespace, ignore_overrides](auto element) { - return static_cast( - this->declare_parameter( - normalized_namespace + element.first, - element.second.first, - element.second.second, - ignore_overrides) - ); - } - ); + return static_cast(this->declare_parameter( + normalized_namespace + element.first, + element.second.first, + element.second.second, + ignore_overrides)); + }); return result; } template -bool -Node::get_parameter(const std::string & name, ParameterT & parameter) const +bool Node::get_parameter(const std::string & name, ParameterT & parameter) const { std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace()); @@ -233,11 +212,8 @@ Node::get_parameter(const std::string & name, ParameterT & parameter) const } template -bool -Node::get_parameter_or( - const std::string & name, - ParameterT & parameter, - const ParameterT & alternative_value) const +bool Node::get_parameter_or( + const std::string & name, ParameterT & parameter, const ParameterT & alternative_value) const { std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace()); @@ -252,10 +228,8 @@ Node::get_parameter_or( // where our concrete type for ParameterT is std::map, but the to-be-determined // type is the value in the map. template -bool -Node::get_parameters( - const std::string & prefix, - std::map & values) const +bool Node::get_parameters( + const std::string & prefix, std::map & values) const { std::map params; bool result = node_parameters_->get_parameters_by_prefix(prefix, params); diff --git a/rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp index cfc5917689..f98da426fc 100644 --- a/rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp @@ -16,8 +16,8 @@ #define RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_ #include -#include #include +#include #include "rclcpp/node_interfaces/node_base_interface.hpp" @@ -48,17 +48,11 @@ struct has_get_node_base_interface { private: template - static constexpr - auto - check(T *)->typename std::is_same< - decltype(std::declval().get_node_base_interface()), - ReturnType - >::type; + static constexpr auto check(T *) -> + typename std::is_same().get_node_base_interface()), ReturnType>::type; template - static constexpr - std::false_type - check(...); + static constexpr std::false_type check(...); public: using type = decltype(check(nullptr)); @@ -66,9 +60,8 @@ struct has_get_node_base_interface }; // If NodeType is a pointer to NodeBaseInterface already (just normal function overload). -inline -rclcpp::node_interfaces::NodeBaseInterface * -get_node_base_interface_from_pointer(rclcpp::node_interfaces::NodeBaseInterface * pointer) +inline rclcpp::node_interfaces::NodeBaseInterface * get_node_base_interface_from_pointer( + rclcpp::node_interfaces::NodeBaseInterface * pointer) { return pointer; } @@ -76,13 +69,13 @@ get_node_base_interface_from_pointer(rclcpp::node_interfaces::NodeBaseInterface // If NodeType has a method called get_node_base_interface() which returns a shared pointer. template< typename NodeType, - typename std::enable_if::type, - std::shared_ptr - >::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeBaseInterface * -get_node_base_interface_from_pointer(NodeType node_pointer) + typename std::enable_if< + has_get_node_base_interface< + typename std::remove_pointer::type, + std::shared_ptr >::value, + int>::type = 0> +rclcpp::node_interfaces::NodeBaseInterface * get_node_base_interface_from_pointer( + NodeType node_pointer) { return node_pointer->get_node_base_interface().get(); } @@ -90,13 +83,13 @@ get_node_base_interface_from_pointer(NodeType node_pointer) // If NodeType has a method called get_node_base_interface() which returns a pointer. template< typename NodeType, - typename std::enable_if::type, - rclcpp::node_interfaces::NodeBaseInterface * - >::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeBaseInterface * -get_node_base_interface_from_pointer(NodeType node_pointer) + typename std::enable_if< + has_get_node_base_interface< + typename std::remove_pointer::type, + rclcpp::node_interfaces::NodeBaseInterface *>::value, + int>::type = 0> +rclcpp::node_interfaces::NodeBaseInterface * get_node_base_interface_from_pointer( + NodeType node_pointer) { return node_pointer->get_node_base_interface(); } @@ -104,13 +97,14 @@ get_node_base_interface_from_pointer(NodeType node_pointer) // Forward shared_ptr's to const node pointer signatures. template< typename NodeType, - typename std::enable_if::type::element_type> * - >::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeBaseInterface * -get_node_base_interface_from_pointer(NodeType node_shared_pointer) + typename std::enable_if< + std::is_same< + NodeType, + typename std::shared_ptr::type::element_type> *>:: + value, + int>::type = 0> +rclcpp::node_interfaces::NodeBaseInterface * get_node_base_interface_from_pointer( + NodeType node_shared_pointer) { return get_node_base_interface_from_pointer(node_shared_pointer->get()); } @@ -120,10 +114,8 @@ get_node_base_interface_from_pointer(NodeType node_shared_pointer) /// Get the NodeBaseInterface as a pointer from a pointer to a "Node like" object. template< typename NodeType, - typename std::enable_if::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeBaseInterface * -get_node_base_interface(NodeType node_pointer) + typename std::enable_if::value, int>::type = 0> +rclcpp::node_interfaces::NodeBaseInterface * get_node_base_interface(NodeType node_pointer) { // Forward pointers to detail implmentation directly. return detail::get_node_base_interface_from_pointer(node_pointer); @@ -133,11 +125,9 @@ get_node_base_interface(NodeType node_pointer) template< typename NodeType, typename std::enable_if< - !std::is_pointer::type>::value, int - >::type = 0 -> -rclcpp::node_interfaces::NodeBaseInterface * -get_node_base_interface(NodeType && node_reference) + !std::is_pointer::type>::value, + int>::type = 0> +rclcpp::node_interfaces::NodeBaseInterface * get_node_base_interface(NodeType && node_reference) { // Forward references to detail implmentation as a pointer. return detail::get_node_base_interface_from_pointer(&node_reference); diff --git a/rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp index 9c54f06e9c..fd99d3408c 100644 --- a/rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp @@ -16,8 +16,8 @@ #define RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_ #include -#include #include +#include #include "rclcpp/node_interfaces/node_timers_interface.hpp" @@ -48,17 +48,11 @@ struct has_get_node_timers_interface { private: template - static constexpr - auto - check(T *)->typename std::is_same< - decltype(std::declval().get_node_timers_interface()), - ReturnType - >::type; + static constexpr auto check(T *) -> typename std:: + is_same().get_node_timers_interface()), ReturnType>::type; template - static constexpr - std::false_type - check(...); + static constexpr std::false_type check(...); public: using type = decltype(check(nullptr)); @@ -66,9 +60,8 @@ struct has_get_node_timers_interface }; // If NodeType is a pointer to NodeTimersInterface already (just normal function overload). -inline -rclcpp::node_interfaces::NodeTimersInterface * -get_node_timers_interface_from_pointer(rclcpp::node_interfaces::NodeTimersInterface * pointer) +inline rclcpp::node_interfaces::NodeTimersInterface * get_node_timers_interface_from_pointer( + rclcpp::node_interfaces::NodeTimersInterface * pointer) { return pointer; } @@ -76,13 +69,13 @@ get_node_timers_interface_from_pointer(rclcpp::node_interfaces::NodeTimersInterf // If NodeType has a method called get_node_timers_interface() which returns a shared pointer. template< typename NodeType, - typename std::enable_if::type, - std::shared_ptr - >::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeTimersInterface * -get_node_timers_interface_from_pointer(NodeType node_pointer) + typename std::enable_if< + has_get_node_timers_interface< + typename std::remove_pointer::type, + std::shared_ptr >::value, + int>::type = 0> +rclcpp::node_interfaces::NodeTimersInterface * get_node_timers_interface_from_pointer( + NodeType node_pointer) { return node_pointer->get_node_timers_interface().get(); } @@ -90,13 +83,13 @@ get_node_timers_interface_from_pointer(NodeType node_pointer) // If NodeType has a method called get_node_timers_interface() which returns a pointer. template< typename NodeType, - typename std::enable_if::type, - rclcpp::node_interfaces::NodeTimersInterface * - >::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeTimersInterface * -get_node_timers_interface_from_pointer(NodeType node_pointer) + typename std::enable_if< + has_get_node_timers_interface< + typename std::remove_pointer::type, + rclcpp::node_interfaces::NodeTimersInterface *>::value, + int>::type = 0> +rclcpp::node_interfaces::NodeTimersInterface * get_node_timers_interface_from_pointer( + NodeType node_pointer) { return node_pointer->get_node_timers_interface(); } @@ -104,13 +97,14 @@ get_node_timers_interface_from_pointer(NodeType node_pointer) // Forward shared_ptr's to const node pointer signatures. template< typename NodeType, - typename std::enable_if::type::element_type> * - >::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeTimersInterface * -get_node_timers_interface_from_pointer(NodeType node_shared_pointer) + typename std::enable_if< + std::is_same< + NodeType, + typename std::shared_ptr::type::element_type> *>:: + value, + int>::type = 0> +rclcpp::node_interfaces::NodeTimersInterface * get_node_timers_interface_from_pointer( + NodeType node_shared_pointer) { return get_node_timers_interface_from_pointer(node_shared_pointer->get()); } @@ -120,10 +114,8 @@ get_node_timers_interface_from_pointer(NodeType node_shared_pointer) /// Get the NodeTimersInterface as a pointer from a pointer to a "Node like" object. template< typename NodeType, - typename std::enable_if::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeTimersInterface * -get_node_timers_interface(NodeType node_pointer) + typename std::enable_if::value, int>::type = 0> +rclcpp::node_interfaces::NodeTimersInterface * get_node_timers_interface(NodeType node_pointer) { // Forward pointers to detail implmentation directly. return detail::get_node_timers_interface_from_pointer(node_pointer); @@ -133,11 +125,9 @@ get_node_timers_interface(NodeType node_pointer) template< typename NodeType, typename std::enable_if< - !std::is_pointer::type>::value, int - >::type = 0 -> -rclcpp::node_interfaces::NodeTimersInterface * -get_node_timers_interface(NodeType && node_reference) + !std::is_pointer::type>::value, + int>::type = 0> +rclcpp::node_interfaces::NodeTimersInterface * get_node_timers_interface(NodeType && node_reference) { // Forward references to detail implmentation as a pointer. return detail::get_node_timers_interface_from_pointer(&node_reference); diff --git a/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp index c84234282f..f60390346d 100644 --- a/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp @@ -16,8 +16,8 @@ #define RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_ #include -#include #include +#include #include "rclcpp/node_interfaces/node_topics_interface.hpp" @@ -48,17 +48,11 @@ struct has_get_node_topics_interface { private: template - static constexpr - auto - check(T *)->typename std::is_same< - decltype(std::declval().get_node_topics_interface()), - ReturnType - >::type; + static constexpr auto check(T *) -> typename std:: + is_same().get_node_topics_interface()), ReturnType>::type; template - static constexpr - std::false_type - check(...); + static constexpr std::false_type check(...); public: using type = decltype(check(nullptr)); @@ -66,9 +60,8 @@ struct has_get_node_topics_interface }; // If NodeType is a pointer to NodeTopicsInterface already (just normal function overload). -inline -rclcpp::node_interfaces::NodeTopicsInterface * -get_node_topics_interface_from_pointer(rclcpp::node_interfaces::NodeTopicsInterface * pointer) +inline rclcpp::node_interfaces::NodeTopicsInterface * get_node_topics_interface_from_pointer( + rclcpp::node_interfaces::NodeTopicsInterface * pointer) { return pointer; } @@ -76,13 +69,13 @@ get_node_topics_interface_from_pointer(rclcpp::node_interfaces::NodeTopicsInterf // If NodeType has a method called get_node_topics_interface() which returns a shared pointer. template< typename NodeType, - typename std::enable_if::type, - std::shared_ptr - >::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeTopicsInterface * -get_node_topics_interface_from_pointer(NodeType node_pointer) + typename std::enable_if< + has_get_node_topics_interface< + typename std::remove_pointer::type, + std::shared_ptr >::value, + int>::type = 0> +rclcpp::node_interfaces::NodeTopicsInterface * get_node_topics_interface_from_pointer( + NodeType node_pointer) { return node_pointer->get_node_topics_interface().get(); } @@ -90,13 +83,13 @@ get_node_topics_interface_from_pointer(NodeType node_pointer) // If NodeType has a method called get_node_topics_interface() which returns a pointer. template< typename NodeType, - typename std::enable_if::type, - rclcpp::node_interfaces::NodeTopicsInterface * - >::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeTopicsInterface * -get_node_topics_interface_from_pointer(NodeType node_pointer) + typename std::enable_if< + has_get_node_topics_interface< + typename std::remove_pointer::type, + rclcpp::node_interfaces::NodeTopicsInterface *>::value, + int>::type = 0> +rclcpp::node_interfaces::NodeTopicsInterface * get_node_topics_interface_from_pointer( + NodeType node_pointer) { return node_pointer->get_node_topics_interface(); } @@ -104,13 +97,14 @@ get_node_topics_interface_from_pointer(NodeType node_pointer) // Forward shared_ptr's to const node pointer signatures. template< typename NodeType, - typename std::enable_if::type::element_type> * - >::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeTopicsInterface * -get_node_topics_interface_from_pointer(NodeType node_shared_pointer) + typename std::enable_if< + std::is_same< + NodeType, + typename std::shared_ptr::type::element_type> *>:: + value, + int>::type = 0> +rclcpp::node_interfaces::NodeTopicsInterface * get_node_topics_interface_from_pointer( + NodeType node_shared_pointer) { return get_node_topics_interface_from_pointer(node_shared_pointer->get()); } @@ -120,10 +114,8 @@ get_node_topics_interface_from_pointer(NodeType node_shared_pointer) /// Get the NodeTopicsInterface as a pointer from a pointer to a "Node like" object. template< typename NodeType, - typename std::enable_if::value, int>::type = 0 -> -rclcpp::node_interfaces::NodeTopicsInterface * -get_node_topics_interface(NodeType node_pointer) + typename std::enable_if::value, int>::type = 0> +rclcpp::node_interfaces::NodeTopicsInterface * get_node_topics_interface(NodeType node_pointer) { // Forward pointers to detail implmentation directly. return detail::get_node_topics_interface_from_pointer(node_pointer); @@ -133,11 +125,9 @@ get_node_topics_interface(NodeType node_pointer) template< typename NodeType, typename std::enable_if< - !std::is_pointer::type>::value, int - >::type = 0 -> -rclcpp::node_interfaces::NodeTopicsInterface * -get_node_topics_interface(NodeType && node_reference) + !std::is_pointer::type>::value, + int>::type = 0> +rclcpp::node_interfaces::NodeTopicsInterface * get_node_topics_interface(NodeType && node_reference) { // Forward references to detail implmentation as a pointer. return detail::get_node_topics_interface_from_pointer(&node_reference); diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 5bede31fe1..aaaecdeb69 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -46,93 +46,60 @@ class NodeBase : public NodeBaseInterface bool use_intra_process_default); RCLCPP_PUBLIC - virtual - ~NodeBase(); + virtual ~NodeBase(); RCLCPP_PUBLIC - virtual - const char * - get_name() const; + virtual const char * get_name() const; RCLCPP_PUBLIC - virtual - const char * - get_namespace() const; + virtual const char * get_namespace() const; RCLCPP_PUBLIC - virtual - const char * - get_fully_qualified_name() const; + virtual const char * get_fully_qualified_name() const; RCLCPP_PUBLIC - virtual - rclcpp::Context::SharedPtr - get_context(); + virtual rclcpp::Context::SharedPtr get_context(); RCLCPP_PUBLIC - virtual - rcl_node_t * - get_rcl_node_handle(); + virtual rcl_node_t * get_rcl_node_handle(); RCLCPP_PUBLIC - virtual - const rcl_node_t * - get_rcl_node_handle() const; + virtual const rcl_node_t * get_rcl_node_handle() const; RCLCPP_PUBLIC - virtual - std::shared_ptr - get_shared_rcl_node_handle(); + virtual std::shared_ptr get_shared_rcl_node_handle(); RCLCPP_PUBLIC - virtual - std::shared_ptr - get_shared_rcl_node_handle() const; + virtual std::shared_ptr get_shared_rcl_node_handle() const; RCLCPP_PUBLIC - virtual - bool - assert_liveliness() const; + virtual bool assert_liveliness() const; RCLCPP_PUBLIC - virtual - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + virtual rclcpp::callback_group::CallbackGroup::SharedPtr create_callback_group( + rclcpp::callback_group::CallbackGroupType group_type); RCLCPP_PUBLIC - virtual - rclcpp::callback_group::CallbackGroup::SharedPtr - get_default_callback_group(); + virtual rclcpp::callback_group::CallbackGroup::SharedPtr get_default_callback_group(); RCLCPP_PUBLIC - virtual - bool - callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group); + virtual bool callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group); RCLCPP_PUBLIC - virtual - const std::vector & - get_callback_groups() const; + virtual const std::vector & get_callback_groups() + const; RCLCPP_PUBLIC - virtual - std::atomic_bool & - get_associated_with_executor_atomic(); + virtual std::atomic_bool & get_associated_with_executor_atomic(); RCLCPP_PUBLIC - virtual - rcl_guard_condition_t * - get_notify_guard_condition(); + virtual rcl_guard_condition_t * get_notify_guard_condition(); RCLCPP_PUBLIC - virtual - std::unique_lock - acquire_notify_guard_condition_lock() const; + virtual std::unique_lock acquire_notify_guard_condition_lock() const; RCLCPP_PUBLIC - virtual - bool - get_use_intra_process_default() const; + virtual bool get_use_intra_process_default() const; private: RCLCPP_DISABLE_COPY(NodeBase) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index d0247ee0f2..004eee43fd 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -39,48 +39,35 @@ class NodeBaseInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface) RCLCPP_PUBLIC - virtual - ~NodeBaseInterface() = default; + virtual ~NodeBaseInterface() = default; /// Return the name of the node. /** \return The name of the node. */ RCLCPP_PUBLIC - virtual - const char * - get_name() const = 0; + virtual const char * get_name() const = 0; /// Return the namespace of the node. /** \return The namespace of the node. */ RCLCPP_PUBLIC - virtual - const char * - get_namespace() const = 0; + virtual const char * get_namespace() const = 0; /// Return the fully qualified name of the node. /** \return The fully qualified name of the node. */ RCLCPP_PUBLIC - virtual - const char * - get_fully_qualified_name() const = 0; + virtual const char * get_fully_qualified_name() const = 0; /// Return the context of the node. /** \return SharedPtr to the node's context. */ RCLCPP_PUBLIC - virtual - rclcpp::Context::SharedPtr - get_context() = 0; + virtual rclcpp::Context::SharedPtr get_context() = 0; /// Return the rcl_node_t node handle (non-const version). RCLCPP_PUBLIC - virtual - rcl_node_t * - get_rcl_node_handle() = 0; + virtual rcl_node_t * get_rcl_node_handle() = 0; /// Return the rcl_node_t node handle (const version). RCLCPP_PUBLIC - virtual - const rcl_node_t * - get_rcl_node_handle() const = 0; + virtual const rcl_node_t * get_rcl_node_handle() const = 0; /// Return the rcl_node_t node handle in a std::shared_ptr. /** @@ -88,9 +75,7 @@ class NodeBaseInterface * The actual rcl node is not finalized until it is out of scope everywhere. */ RCLCPP_PUBLIC - virtual - std::shared_ptr - get_shared_rcl_node_handle() = 0; + virtual std::shared_ptr get_shared_rcl_node_handle() = 0; /// Return the rcl_node_t node handle in a std::shared_ptr. /** @@ -98,45 +83,33 @@ class NodeBaseInterface * The actual rcl node is not finalized until it is out of scope everywhere. */ RCLCPP_PUBLIC - virtual - std::shared_ptr - get_shared_rcl_node_handle() const = 0; + virtual std::shared_ptr get_shared_rcl_node_handle() const = 0; /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE). RCLCPP_PUBLIC - virtual - bool - assert_liveliness() const = 0; + virtual bool assert_liveliness() const = 0; /// Create and return a callback group. RCLCPP_PUBLIC - virtual - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) = 0; + virtual rclcpp::callback_group::CallbackGroup::SharedPtr create_callback_group( + rclcpp::callback_group::CallbackGroupType group_type) = 0; /// Return the default callback group. RCLCPP_PUBLIC - virtual - rclcpp::callback_group::CallbackGroup::SharedPtr - get_default_callback_group() = 0; + virtual rclcpp::callback_group::CallbackGroup::SharedPtr get_default_callback_group() = 0; /// Return true if the given callback group is associated with this node. RCLCPP_PUBLIC - virtual - bool - callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + virtual bool callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; /// Return list of callback groups associated with this node. RCLCPP_PUBLIC - virtual - const std::vector & - get_callback_groups() const = 0; + virtual const std::vector & get_callback_groups() + const = 0; /// Return the atomic bool which is used to ensure only one executor is used. RCLCPP_PUBLIC - virtual - std::atomic_bool & - get_associated_with_executor_atomic() = 0; + virtual std::atomic_bool & get_associated_with_executor_atomic() = 0; /// Return guard condition that should be notified when the internal node state changes. /** @@ -145,22 +118,16 @@ class NodeBaseInterface * \return the rcl_guard_condition_t if it is valid, else nullptr */ RCLCPP_PUBLIC - virtual - rcl_guard_condition_t * - get_notify_guard_condition() = 0; + virtual rcl_guard_condition_t * get_notify_guard_condition() = 0; /// Acquire and return a scoped lock that protects the notify guard condition. /** This should be used when triggering the notify guard condition. */ RCLCPP_PUBLIC - virtual - std::unique_lock - acquire_notify_guard_condition_lock() const = 0; + virtual std::unique_lock acquire_notify_guard_condition_lock() const = 0; /// Return the default preference for using intra process communication. RCLCPP_PUBLIC - virtual - bool - get_use_intra_process_default() const = 0; + virtual bool get_use_intra_process_default() const = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp index e0e7059b14..a73092c6fd 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp @@ -45,14 +45,11 @@ class NodeClock : public NodeClockInterface rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging); RCLCPP_PUBLIC - virtual - ~NodeClock(); + virtual ~NodeClock(); /// Get a clock which will be kept up to date by the node. RCLCPP_PUBLIC - virtual - rclcpp::Clock::SharedPtr - get_clock(); + virtual rclcpp::Clock::SharedPtr get_clock(); private: RCLCPP_DISABLE_COPY(NodeClock) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp index 055d475c13..7933cc165b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp @@ -32,14 +32,11 @@ class NodeClockInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface) RCLCPP_PUBLIC - virtual - ~NodeClockInterface() = default; + virtual ~NodeClockInterface() = default; /// Get a ROS clock which will be kept up to date by the node. RCLCPP_PUBLIC - virtual - rclcpp::Clock::SharedPtr - get_clock() = 0; + virtual rclcpp::Clock::SharedPtr get_clock() = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index 8ecaf074f2..8ceb833402 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -52,70 +52,45 @@ class NodeGraph : public NodeGraphInterface explicit NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base); RCLCPP_PUBLIC - virtual - ~NodeGraph(); + virtual ~NodeGraph(); RCLCPP_PUBLIC - virtual - std::map> - get_topic_names_and_types(bool no_demangle = false) const; + virtual std::map> get_topic_names_and_types( + bool no_demangle = false) const; RCLCPP_PUBLIC - virtual - std::map> - get_service_names_and_types() const; + virtual std::map> get_service_names_and_types() const; RCLCPP_PUBLIC - virtual - std::vector - get_node_names() const; + virtual std::vector get_node_names() const; RCLCPP_PUBLIC - virtual - std::vector> - get_node_names_and_namespaces() const; + virtual std::vector> get_node_names_and_namespaces() const; RCLCPP_PUBLIC - virtual - size_t - count_publishers(const std::string & topic_name) const; + virtual size_t count_publishers(const std::string & topic_name) const; RCLCPP_PUBLIC - virtual - size_t - count_subscribers(const std::string & topic_name) const; + virtual size_t count_subscribers(const std::string & topic_name) const; RCLCPP_PUBLIC - virtual - const rcl_guard_condition_t * - get_graph_guard_condition() const; + virtual const rcl_guard_condition_t * get_graph_guard_condition() const; RCLCPP_PUBLIC - virtual - void - notify_graph_change(); + virtual void notify_graph_change(); RCLCPP_PUBLIC - virtual - void - notify_shutdown(); + virtual void notify_shutdown(); RCLCPP_PUBLIC - virtual - rclcpp::Event::SharedPtr - get_graph_event(); + virtual rclcpp::Event::SharedPtr get_graph_event(); RCLCPP_PUBLIC - virtual - void - wait_for_graph_change( - rclcpp::Event::SharedPtr event, - std::chrono::nanoseconds timeout); + virtual void wait_for_graph_change( + rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout); RCLCPP_PUBLIC - virtual - size_t - count_graph_users(); + virtual size_t count_graph_users(); private: RCLCPP_DISABLE_COPY(NodeGraph) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 022984dd47..f3396f5fc4 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -39,8 +39,7 @@ class NodeGraphInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface) RCLCPP_PUBLIC - virtual - ~NodeGraphInterface() = default; + virtual ~NodeGraphInterface() = default; /// Return a map of existing topic names to list of topic types. /** @@ -50,9 +49,8 @@ class NodeGraphInterface * \param[in] no_demangle if true, topic names and types are not demangled */ RCLCPP_PUBLIC - virtual - std::map> - get_topic_names_and_types(bool no_demangle = false) const = 0; + virtual std::map> get_topic_names_and_types( + bool no_demangle = false) const = 0; /// Return a map of existing service names to list of service types. /** @@ -61,39 +59,28 @@ class NodeGraphInterface * process. */ RCLCPP_PUBLIC - virtual - std::map> - get_service_names_and_types() const = 0; + virtual std::map> get_service_names_and_types() const = 0; /// Return a vector of existing node names (string). RCLCPP_PUBLIC - virtual - std::vector - get_node_names() const = 0; + virtual std::vector get_node_names() const = 0; /// Return a vector of existing node names and namespaces (pair of string). RCLCPP_PUBLIC - virtual - std::vector> - get_node_names_and_namespaces() const = 0; + virtual std::vector> get_node_names_and_namespaces() + const = 0; /// Return the number of publishers that are advertised on a given topic. RCLCPP_PUBLIC - virtual - size_t - count_publishers(const std::string & topic_name) const = 0; + virtual size_t count_publishers(const std::string & topic_name) const = 0; /// Return the number of subscribers who have created a subscription for a given topic. RCLCPP_PUBLIC - virtual - size_t - count_subscribers(const std::string & topic_name) const = 0; + virtual size_t count_subscribers(const std::string & topic_name) const = 0; /// Return the rcl guard condition which is triggered when the ROS graph changes. RCLCPP_PUBLIC - virtual - const rcl_guard_condition_t * - get_graph_guard_condition() const = 0; + virtual const rcl_guard_condition_t * get_graph_guard_condition() const = 0; /// Notify threads waiting on graph changes. /** @@ -106,15 +93,11 @@ class NodeGraphInterface * \throws RCLBaseError (a child of that exception) when an rcl error occurs */ RCLCPP_PUBLIC - virtual - void - notify_graph_change() = 0; + virtual void notify_graph_change() = 0; /// Notify any and all blocking node actions that shutdown has occurred. RCLCPP_PUBLIC - virtual - void - notify_shutdown() = 0; + virtual void notify_shutdown() = 0; /// Return a graph event, which will be set anytime a graph change occurs. /** @@ -123,9 +106,7 @@ class NodeGraphInterface * out of scope. */ RCLCPP_PUBLIC - virtual - rclcpp::Event::SharedPtr - get_graph_event() = 0; + virtual rclcpp::Event::SharedPtr get_graph_event() = 0; /// Wait for a graph event to occur by waiting on an Event to become set. /** @@ -136,20 +117,15 @@ class NodeGraphInterface * get_graph_event(). */ RCLCPP_PUBLIC - virtual - void - wait_for_graph_change( - rclcpp::Event::SharedPtr event, - std::chrono::nanoseconds timeout) = 0; + virtual void wait_for_graph_change( + rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout) = 0; /// Return the number of on loan graph events, see get_graph_event(). /** * This is typically only used by the rclcpp::graph_listener::GraphListener. */ RCLCPP_PUBLIC - virtual - size_t - count_graph_users() = 0; + virtual size_t count_graph_users() = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp index 9694bf377a..11b4c553a9 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp @@ -38,18 +38,13 @@ class NodeLogging : public NodeLoggingInterface explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base); RCLCPP_PUBLIC - virtual - ~NodeLogging(); + virtual ~NodeLogging(); RCLCPP_PUBLIC - virtual - rclcpp::Logger - get_logger() const; + virtual rclcpp::Logger get_logger() const; RCLCPP_PUBLIC - virtual - const char * - get_logger_name() const; + virtual const char * get_logger_name() const; private: RCLCPP_DISABLE_COPY(NodeLogging) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp index ace0afcffe..4ca7c2343f 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp @@ -34,22 +34,17 @@ class NodeLoggingInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface) RCLCPP_PUBLIC - virtual - ~NodeLoggingInterface() = default; + virtual ~NodeLoggingInterface() = default; /// Return the logger of the node. /** \return The logger of the node. */ RCLCPP_PUBLIC - virtual - rclcpp::Logger - get_logger() const = 0; + virtual rclcpp::Logger get_logger() const = 0; /// Return the logger name associated with the node. /** \return The logger name associated with the node. */ RCLCPP_PUBLIC - virtual - const char * - get_logger_name() const = 0; + virtual const char * get_logger_name() const = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 4da6349e30..0f985b2604 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -15,9 +15,9 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ +#include #include #include -#include #include #include @@ -55,12 +55,11 @@ struct ParameterInfo class ParameterMutationRecursionGuard { public: - explicit ParameterMutationRecursionGuard(bool & allow_mod) - : allow_modification_(allow_mod) + explicit ParameterMutationRecursionGuard(bool & allow_mod) : allow_modification_(allow_mod) { if (!allow_modification_) { throw rclcpp::exceptions::ParameterModifiedInCallbackException( - "cannot set or declare a parameter, or change the callback from within set callback"); + "cannot set or declare a parameter, or change the callback from within set callback"); } allow_modification_ = false; @@ -97,82 +96,69 @@ class NodeParameters : public NodeParametersInterface bool automatically_declare_parameters_from_overrides); RCLCPP_PUBLIC - virtual - ~NodeParameters(); + virtual ~NodeParameters(); RCLCPP_PUBLIC - const rclcpp::ParameterValue & - declare_parameter( + const rclcpp::ParameterValue & declare_parameter( const std::string & name, const rclcpp::ParameterValue & default_value, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, bool ignore_override) override; RCLCPP_PUBLIC - void - undeclare_parameter(const std::string & name) override; + void undeclare_parameter(const std::string & name) override; RCLCPP_PUBLIC - bool - has_parameter(const std::string & name) const override; + bool has_parameter(const std::string & name) const override; RCLCPP_PUBLIC - std::vector - set_parameters( + std::vector set_parameters( const std::vector & parameters) override; RCLCPP_PUBLIC - rcl_interfaces::msg::SetParametersResult - set_parameters_atomically( + rcl_interfaces::msg::SetParametersResult set_parameters_atomically( const std::vector & parameters) override; RCLCPP_PUBLIC - std::vector - get_parameters(const std::vector & names) const override; + std::vector get_parameters( + const std::vector & names) const override; RCLCPP_PUBLIC - rclcpp::Parameter - get_parameter(const std::string & name) const override; + rclcpp::Parameter get_parameter(const std::string & name) const override; RCLCPP_PUBLIC - bool - get_parameter( - const std::string & name, - rclcpp::Parameter & parameter) const override; + bool get_parameter(const std::string & name, rclcpp::Parameter & parameter) const override; RCLCPP_PUBLIC - bool - get_parameters_by_prefix( + bool get_parameters_by_prefix( const std::string & prefix, std::map & parameters) const override; RCLCPP_PUBLIC - std::vector - describe_parameters(const std::vector & names) const override; + std::vector describe_parameters( + const std::vector & names) const override; RCLCPP_PUBLIC - std::vector - get_parameter_types(const std::vector & names) const override; + std::vector get_parameter_types(const std::vector & names) const override; RCLCPP_PUBLIC - rcl_interfaces::msg::ListParametersResult - list_parameters(const std::vector & prefixes, uint64_t depth) const override; + rcl_interfaces::msg::ListParametersResult list_parameters( + const std::vector & prefixes, uint64_t depth) const override; RCLCPP_PUBLIC - OnSetParametersCallbackHandle::SharedPtr - add_on_set_parameters_callback(OnParametersSetCallbackType callback) override; + OnSetParametersCallbackHandle::SharedPtr add_on_set_parameters_callback( + OnParametersSetCallbackType callback) override; RCLCPP_PUBLIC - void - remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override; + void remove_on_set_parameters_callback( + const OnSetParametersCallbackHandle * const handler) override; RCLCPP_PUBLIC - OnParametersSetCallbackType - set_on_parameters_set_callback(OnParametersSetCallbackType callback) override; + OnParametersSetCallbackType set_on_parameters_set_callback( + OnParametersSetCallbackType callback) override; RCLCPP_PUBLIC - const std::map & - get_parameter_overrides() const override; + const std::map & get_parameter_overrides() const override; using CallbacksContainerType = std::list; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index f1102c0afd..15cef5fac8 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -38,9 +38,7 @@ struct OnSetParametersCallbackHandle RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle) using OnParametersSetCallbackType = - std::function< - rcl_interfaces::msg::SetParametersResult( - const std::vector &)>; + std::function &)>; OnParametersSetCallbackType callback; }; @@ -52,21 +50,18 @@ class NodeParametersInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface) RCLCPP_PUBLIC - virtual - ~NodeParametersInterface() = default; + virtual ~NodeParametersInterface() = default; /// Declare and initialize a parameter. /** * \sa rclcpp::Node::declare_parameter */ RCLCPP_PUBLIC - virtual - const rclcpp::ParameterValue & - declare_parameter( + virtual const rclcpp::ParameterValue & declare_parameter( const std::string & name, const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(), const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor(), + rcl_interfaces::msg::ParameterDescriptor(), bool ignore_override = false) = 0; /// Undeclare a parameter. @@ -74,36 +69,29 @@ class NodeParametersInterface * \sa rclcpp::Node::undeclare_parameter */ RCLCPP_PUBLIC - virtual - void - undeclare_parameter(const std::string & name) = 0; + virtual void undeclare_parameter(const std::string & name) = 0; /// Return true if the parameter has been declared, otherwise false. /** * \sa rclcpp::Node::has_parameter */ RCLCPP_PUBLIC - virtual - bool - has_parameter(const std::string & name) const = 0; + virtual bool has_parameter(const std::string & name) const = 0; /// Set one or more parameters, one at a time. /** * \sa rclcpp::Node::set_parameters */ RCLCPP_PUBLIC - virtual - std::vector - set_parameters(const std::vector & parameters) = 0; + virtual std::vector set_parameters( + const std::vector & parameters) = 0; /// Set and initialize a parameter, all at once. /** * \sa rclcpp::Node::set_parameters_atomically */ RCLCPP_PUBLIC - virtual - rcl_interfaces::msg::SetParametersResult - set_parameters_atomically( + virtual rcl_interfaces::msg::SetParametersResult set_parameters_atomically( const std::vector & parameters) = 0; /// Get descriptions of parameters given their names. @@ -113,9 +101,8 @@ class NodeParametersInterface * Any parameter not found is omitted from the returned list. */ RCLCPP_PUBLIC - virtual - std::vector - get_parameters(const std::vector & names) const = 0; + virtual std::vector get_parameters( + const std::vector & names) const = 0; /// Get the description of one parameter given a name. /* @@ -124,9 +111,7 @@ class NodeParametersInterface * \throws std::out_of_range if the parameter does not exist on the node. */ RCLCPP_PUBLIC - virtual - rclcpp::Parameter - get_parameter(const std::string & name) const = 0; + virtual rclcpp::Parameter get_parameter(const std::string & name) const = 0; /// Get the description of one parameter given a name. /* @@ -136,11 +121,7 @@ class NodeParametersInterface * \return false if the parameter does not exist. */ RCLCPP_PUBLIC - virtual - bool - get_parameter( - const std::string & name, - rclcpp::Parameter & parameter) const = 0; + virtual bool get_parameter(const std::string & name, rclcpp::Parameter & parameter) const = 0; /// Get all parameters that have the specified prefix into the parameters map. /* @@ -150,26 +131,20 @@ class NodeParametersInterface * \return false otherwise. */ RCLCPP_PUBLIC - virtual - bool - get_parameters_by_prefix( - const std::string & prefix, - std::map & parameters) const = 0; + virtual bool get_parameters_by_prefix( + const std::string & prefix, std::map & parameters) const = 0; RCLCPP_PUBLIC - virtual - std::vector - describe_parameters(const std::vector & names) const = 0; + virtual std::vector describe_parameters( + const std::vector & names) const = 0; RCLCPP_PUBLIC - virtual - std::vector - get_parameter_types(const std::vector & names) const = 0; + virtual std::vector get_parameter_types( + const std::vector & names) const = 0; RCLCPP_PUBLIC - virtual - rcl_interfaces::msg::ListParametersResult - list_parameters(const std::vector & prefixes, uint64_t depth) const = 0; + virtual rcl_interfaces::msg::ListParametersResult list_parameters( + const std::vector & prefixes, uint64_t depth) const = 0; using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType; @@ -178,33 +153,28 @@ class NodeParametersInterface * \sa rclcpp::Node::add_on_set_parameters_callback */ RCLCPP_PUBLIC - virtual - OnSetParametersCallbackHandle::SharedPtr - add_on_set_parameters_callback(OnParametersSetCallbackType callback) = 0; + virtual OnSetParametersCallbackHandle::SharedPtr add_on_set_parameters_callback( + OnParametersSetCallbackType callback) = 0; /// Remove a callback registered with `add_on_set_parameters_callback`. /** * \sa rclcpp::Node::remove_on_set_parameters_callback */ RCLCPP_PUBLIC - virtual - void - remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0; + virtual void remove_on_set_parameters_callback( + const OnSetParametersCallbackHandle * const handler) = 0; /// Register a callback for when parameters are being set, return an existing one. /** * \sa rclcpp::Node::set_on_parameters_set_callback */ RCLCPP_PUBLIC - virtual - OnParametersSetCallbackType - set_on_parameters_set_callback(OnParametersSetCallbackType callback) = 0; + virtual OnParametersSetCallbackType set_on_parameters_set_callback( + OnParametersSetCallbackType callback) = 0; /// Return the initial parameter values used by the NodeParameters to override default values. RCLCPP_PUBLIC - virtual - const std::map & - get_parameter_overrides() const = 0; + virtual const std::map & get_parameter_overrides() const = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp index 0b0d99dceb..58d9e6f7e4 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp @@ -38,20 +38,15 @@ class NodeServices : public NodeServicesInterface explicit NodeServices(rclcpp::node_interfaces::NodeBaseInterface * node_base); RCLCPP_PUBLIC - virtual - ~NodeServices(); + virtual ~NodeServices(); RCLCPP_PUBLIC - virtual - void - add_client( + virtual void add_client( rclcpp::ClientBase::SharedPtr client_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group); RCLCPP_PUBLIC - virtual - void - add_service( + virtual void add_service( rclcpp::ServiceBase::SharedPtr service_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group); diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp index c75fec865f..a0c9f82bf5 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp @@ -33,20 +33,15 @@ class NodeServicesInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServicesInterface) RCLCPP_PUBLIC - virtual - ~NodeServicesInterface() = default; + virtual ~NodeServicesInterface() = default; RCLCPP_PUBLIC - virtual - void - add_client( + virtual void add_client( rclcpp::ClientBase::SharedPtr client_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; RCLCPP_PUBLIC - virtual - void - add_service( + virtual void add_service( rclcpp::ServiceBase::SharedPtr service_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; }; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp index 6ba37b78e1..a19d0e2402 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp @@ -45,12 +45,10 @@ class NodeTimeSource : public NodeTimeSourceInterface rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters - ); + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters); RCLCPP_PUBLIC - virtual - ~NodeTimeSource(); + virtual ~NodeTimeSource(); private: RCLCPP_DISABLE_COPY(NodeTimeSource) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp index 0f2df14333..88c1ce20c6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp @@ -32,8 +32,7 @@ class NodeTimeSourceInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSourceInterface) RCLCPP_PUBLIC - virtual - ~NodeTimeSourceInterface() = default; + virtual ~NodeTimeSourceInterface() = default; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp index 955e39cbff..b2a8adbc4e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp @@ -37,14 +37,11 @@ class NodeTimers : public NodeTimersInterface explicit NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base); RCLCPP_PUBLIC - virtual - ~NodeTimers(); + virtual ~NodeTimers(); /// Add a timer to the node. RCLCPP_PUBLIC - virtual - void - add_timer( + virtual void add_timer( rclcpp::TimerBase::SharedPtr timer, rclcpp::callback_group::CallbackGroup::SharedPtr callback_group); diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp index 422409af57..b37011ed5f 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp @@ -32,14 +32,11 @@ class NodeTimersInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimersInterface) RCLCPP_PUBLIC - virtual - ~NodeTimersInterface() = default; + virtual ~NodeTimersInterface() = default; /// Add a timer to the node. RCLCPP_PUBLIC - virtual - void - add_timer( + virtual void add_timer( rclcpp::TimerBase::SharedPtr timer, rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; }; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index a36c8e6bc4..76f4a0645d 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -45,36 +45,31 @@ class NodeTopics : public NodeTopicsInterface ~NodeTopics() override; RCLCPP_PUBLIC - rclcpp::PublisherBase::SharedPtr - create_publisher( + rclcpp::PublisherBase::SharedPtr create_publisher( const std::string & topic_name, const rclcpp::PublisherFactory & publisher_factory, const rcl_publisher_options_t & publisher_options, bool use_intra_process) override; RCLCPP_PUBLIC - void - add_publisher( + void add_publisher( rclcpp::PublisherBase::SharedPtr publisher, rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override; RCLCPP_PUBLIC - rclcpp::SubscriptionBase::SharedPtr - create_subscription( + rclcpp::SubscriptionBase::SharedPtr create_subscription( const std::string & topic_name, const rclcpp::SubscriptionFactory & subscription_factory, const rcl_subscription_options_t & subscription_options, bool use_intra_process) override; RCLCPP_PUBLIC - void - add_subscription( + void add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override; RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeBaseInterface * - get_node_base_interface() const override; + rclcpp::node_interfaces::NodeBaseInterface * get_node_base_interface() const override; private: RCLCPP_DISABLE_COPY(NodeTopics) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index 7e70dc067f..17e220e789 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -41,45 +41,34 @@ class NodeTopicsInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface) RCLCPP_PUBLIC - virtual - ~NodeTopicsInterface() = default; + virtual ~NodeTopicsInterface() = default; RCLCPP_PUBLIC - virtual - rclcpp::PublisherBase::SharedPtr - create_publisher( + virtual rclcpp::PublisherBase::SharedPtr create_publisher( const std::string & topic_name, const rclcpp::PublisherFactory & publisher_factory, const rcl_publisher_options_t & publisher_options, bool use_intra_process) = 0; RCLCPP_PUBLIC - virtual - void - add_publisher( + virtual void add_publisher( rclcpp::PublisherBase::SharedPtr publisher, rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; RCLCPP_PUBLIC - virtual - rclcpp::SubscriptionBase::SharedPtr - create_subscription( + virtual rclcpp::SubscriptionBase::SharedPtr create_subscription( const std::string & topic_name, const rclcpp::SubscriptionFactory & subscription_factory, const rcl_subscription_options_t & subscription_options, bool use_intra_process) = 0; RCLCPP_PUBLIC - virtual - void - add_subscription( + virtual void add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; RCLCPP_PUBLIC - virtual - rclcpp::node_interfaces::NodeBaseInterface * - get_node_base_interface() const = 0; + virtual rclcpp::node_interfaces::NodeBaseInterface * get_node_base_interface() const = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp index e16ff8efe2..bab62afec5 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp @@ -20,8 +20,8 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_waitables_interface.hpp" -#include "rclcpp/waitable.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" namespace rclcpp { @@ -38,20 +38,15 @@ class NodeWaitables : public NodeWaitablesInterface explicit NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base); RCLCPP_PUBLIC - virtual - ~NodeWaitables(); + virtual ~NodeWaitables(); RCLCPP_PUBLIC - virtual - void - add_waitable( + virtual void add_waitable( rclcpp::Waitable::SharedPtr waitable_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group); RCLCPP_PUBLIC - virtual - void - remove_waitable( + virtual void remove_waitable( rclcpp::Waitable::SharedPtr waitable_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp index e848cea286..1b258b9e16 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp @@ -32,21 +32,16 @@ class NodeWaitablesInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitablesInterface) RCLCPP_PUBLIC - virtual - ~NodeWaitablesInterface() = default; + virtual ~NodeWaitablesInterface() = default; RCLCPP_PUBLIC - virtual - void - add_waitable( + virtual void add_waitable( rclcpp::Waitable::SharedPtr waitable_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; /// \note this function should not throw because it may be called in destructors RCLCPP_PUBLIC - virtual - void - remove_waitable( + virtual void remove_waitable( rclcpp::Waitable::SharedPtr waitable_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept = 0; }; diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 43141b6bdc..73af816856 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -59,8 +59,7 @@ class NodeOptions /// Destructor. RCLCPP_PUBLIC - virtual - ~NodeOptions() = default; + virtual ~NodeOptions() = default; /// Copy constructor. RCLCPP_PUBLIC @@ -68,8 +67,7 @@ class NodeOptions /// Assignment operator. RCLCPP_PUBLIC - NodeOptions & - operator=(const NodeOptions & other); + NodeOptions & operator=(const NodeOptions & other); /// Return the rcl_node_options used by the node. /** @@ -78,23 +76,19 @@ class NodeOptions * changed, like arguments, use_global_arguments, or the rcl allocator. */ RCLCPP_PUBLIC - const rcl_node_options_t * - get_rcl_node_options() const; + const rcl_node_options_t * get_rcl_node_options() const; /// Return the context to be used by the node. RCLCPP_PUBLIC - rclcpp::Context::SharedPtr - context() const; + rclcpp::Context::SharedPtr context() const; /// Set the context, return this for parameter idiom. RCLCPP_PUBLIC - NodeOptions & - context(rclcpp::Context::SharedPtr context); + NodeOptions & context(rclcpp::Context::SharedPtr context); /// Return a reference to the list of arguments for the node. RCLCPP_PUBLIC - const std::vector & - arguments() const; + const std::vector & arguments() const; /// Set the arguments, return this for parameter idiom. /** @@ -104,17 +98,14 @@ class NodeOptions * This will cause the internal rcl_node_options_t struct to be invalidated. */ RCLCPP_PUBLIC - NodeOptions & - arguments(const std::vector & arguments); + NodeOptions & arguments(const std::vector & arguments); /// Return a reference to the list of parameter overrides. RCLCPP_PUBLIC - std::vector & - parameter_overrides(); + std::vector & parameter_overrides(); RCLCPP_PUBLIC - const std::vector & - parameter_overrides() const; + const std::vector & parameter_overrides() const; /// Set the parameters overrides, return this for parameter idiom. /** @@ -123,13 +114,11 @@ class NodeOptions * values if necessary. */ RCLCPP_PUBLIC - NodeOptions & - parameter_overrides(const std::vector & parameter_overrides); + NodeOptions & parameter_overrides(const std::vector & parameter_overrides); /// Append a single parameter override, parameter idiom style. template - NodeOptions & - append_parameter_override(const std::string & name, const ParameterT & value) + NodeOptions & append_parameter_override(const std::string & name, const ParameterT & value) { this->parameter_overrides().emplace_back(name, rclcpp::ParameterValue(value)); return *this; @@ -137,8 +126,7 @@ class NodeOptions /// Return the use_global_arguments flag. RCLCPP_PUBLIC - bool - use_global_arguments() const; + bool use_global_arguments() const; /// Set the use_global_arguments flag, return this for parameter idiom. /** @@ -149,13 +137,11 @@ class NodeOptions * This will cause the internal rcl_node_options_t struct to be invalidated. */ RCLCPP_PUBLIC - NodeOptions & - use_global_arguments(bool use_global_arguments); + NodeOptions & use_global_arguments(bool use_global_arguments); /// Return the use_intra_process_comms flag. RCLCPP_PUBLIC - bool - use_intra_process_comms() const; + bool use_intra_process_comms() const; /// Set the use_intra_process_comms flag, return this for parameter idiom. /** @@ -168,13 +154,11 @@ class NodeOptions * desirable. */ RCLCPP_PUBLIC - NodeOptions & - use_intra_process_comms(bool use_intra_process_comms); + NodeOptions & use_intra_process_comms(bool use_intra_process_comms); /// Return the start_parameter_services flag. RCLCPP_PUBLIC - bool - start_parameter_services() const; + bool start_parameter_services() const; /// Set the start_parameter_services flag, return this for parameter idiom. /** @@ -187,13 +171,11 @@ class NodeOptions * \sa start_parameter_event_publisher() */ RCLCPP_PUBLIC - NodeOptions & - start_parameter_services(bool start_parameter_services); + NodeOptions & start_parameter_services(bool start_parameter_services); /// Return the start_parameter_event_publisher flag. RCLCPP_PUBLIC - bool - start_parameter_event_publisher() const; + bool start_parameter_event_publisher() const; /// Set the start_parameter_event_publisher flag, return this for parameter idiom. /** @@ -203,26 +185,22 @@ class NodeOptions * separately from the other parameter services. */ RCLCPP_PUBLIC - NodeOptions & - start_parameter_event_publisher(bool start_parameter_event_publisher); + NodeOptions & start_parameter_event_publisher(bool start_parameter_event_publisher); /// Return a reference to the parameter_event_qos QoS. RCLCPP_PUBLIC - const rclcpp::QoS & - parameter_event_qos() const; + const rclcpp::QoS & parameter_event_qos() const; /// Set the parameter_event_qos QoS, return this for parameter idiom. /** * The QoS settings to be used for the parameter event publisher, if enabled. */ RCLCPP_PUBLIC - NodeOptions & - parameter_event_qos(const rclcpp::QoS & parameter_event_qos); + NodeOptions & parameter_event_qos(const rclcpp::QoS & parameter_event_qos); /// Return a reference to the parameter_event_publisher_options. RCLCPP_PUBLIC - const rclcpp::PublisherOptionsBase & - parameter_event_publisher_options() const; + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options() const; /// Set the parameter_event_publisher_options, return this for parameter idiom. /** @@ -233,14 +211,12 @@ class NodeOptions * NodeOptions to also be templated based on the Allocator type. */ RCLCPP_PUBLIC - NodeOptions & - parameter_event_publisher_options( + NodeOptions & parameter_event_publisher_options( const rclcpp::PublisherOptionsBase & parameter_event_publisher_options); /// Return the allow_undeclared_parameters flag. RCLCPP_PUBLIC - bool - allow_undeclared_parameters() const; + bool allow_undeclared_parameters() const; /// Set the allow_undeclared_parameters, return this for parameter idiom. /** @@ -253,13 +229,11 @@ class NodeOptions * any parameter overrides. */ RCLCPP_PUBLIC - NodeOptions & - allow_undeclared_parameters(bool allow_undeclared_parameters); + NodeOptions & allow_undeclared_parameters(bool allow_undeclared_parameters); /// Return the automatically_declare_parameters_from_overrides flag. RCLCPP_PUBLIC - bool - automatically_declare_parameters_from_overrides() const; + bool automatically_declare_parameters_from_overrides() const; /// Set the automatically_declare_parameters_from_overrides, return this. /** @@ -273,27 +247,23 @@ class NodeOptions * declared in this way will use the default constructed ParameterDescriptor. */ RCLCPP_PUBLIC - NodeOptions & - automatically_declare_parameters_from_overrides( + NodeOptions & automatically_declare_parameters_from_overrides( bool automatically_declare_parameters_from_overrides); /// Return the rcl_allocator_t to be used. RCLCPP_PUBLIC - const rcl_allocator_t & - allocator() const; + const rcl_allocator_t & allocator() const; /// Set the rcl_allocator_t to be used, may cause deallocation of existing rcl_node_options_t. /** * This will cause the internal rcl_node_options_t struct to be invalidated. */ RCLCPP_PUBLIC - NodeOptions & - allocator(rcl_allocator_t allocator); + NodeOptions & allocator(rcl_allocator_t allocator); protected: /// Retrieve the ROS_DOMAIN_ID environment variable and populate options. - size_t - get_domain_id_from_env() const; + size_t get_domain_id_from_env() const; private: // This is mutable to allow for a const accessor which lazily creates the node options instance. @@ -303,32 +273,31 @@ class NodeOptions // IMPORTANT: if any of these default values are changed, please update the // documentation in this class. - rclcpp::Context::SharedPtr context_ { + rclcpp::Context::SharedPtr context_{ rclcpp::contexts::default_context::get_global_default_context()}; - std::vector arguments_ {}; + std::vector arguments_{}; - std::vector parameter_overrides_ {}; + std::vector parameter_overrides_{}; - bool use_global_arguments_ {true}; + bool use_global_arguments_{true}; - bool use_intra_process_comms_ {false}; + bool use_intra_process_comms_{false}; - bool start_parameter_services_ {true}; + bool start_parameter_services_{true}; - bool start_parameter_event_publisher_ {true}; + bool start_parameter_event_publisher_{true}; rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS( - rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) - ); + rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)); rclcpp::PublisherOptionsBase parameter_event_publisher_options_ = rclcpp::PublisherOptionsBase(); - bool allow_undeclared_parameters_ {false}; + bool allow_undeclared_parameters_{false}; - bool automatically_declare_parameters_from_overrides_ {false}; + bool automatically_declare_parameters_from_overrides_{false}; - rcl_allocator_t allocator_ {rcl_get_default_allocator()}; + rcl_allocator_t allocator_{rcl_get_default_allocator()}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index cee5585bea..1140699aad 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -42,8 +42,7 @@ namespace detail // class method, so instead we specialize this template function and call it // from the unspecialized, but dependent, class method. template -auto -get_value_helper(const rclcpp::Parameter * parameter); +auto get_value_helper(const rclcpp::Parameter * parameter); } // namespace detail @@ -65,104 +64,83 @@ class Parameter /// Construct with given name and given parameter value. template - Parameter(const std::string & name, ValueTypeT value) - : Parameter(name, ParameterValue(value)) - {} + Parameter(const std::string & name, ValueTypeT value) : Parameter(name, ParameterValue(value)) + { + } RCLCPP_PUBLIC explicit Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_info); /// Equal operator. RCLCPP_PUBLIC - bool - operator==(const Parameter & rhs) const; + bool operator==(const Parameter & rhs) const; /// Not equal operator. RCLCPP_PUBLIC - bool - operator!=(const Parameter & rhs) const; + bool operator!=(const Parameter & rhs) const; RCLCPP_PUBLIC - ParameterType - get_type() const; + ParameterType get_type() const; RCLCPP_PUBLIC - std::string - get_type_name() const; + std::string get_type_name() const; RCLCPP_PUBLIC - const std::string & - get_name() const; + const std::string & get_name() const; RCLCPP_PUBLIC - rcl_interfaces::msg::ParameterValue - get_value_message() const; + rcl_interfaces::msg::ParameterValue get_value_message() const; /// Get the internal storage for the parameter value. RCLCPP_PUBLIC - const rclcpp::ParameterValue & - get_parameter_value() const; + const rclcpp::ParameterValue & get_parameter_value() const; /// Get value of parameter using rclcpp::ParameterType as template argument. template - decltype(auto) - get_value() const + decltype(auto) get_value() const { return value_.get(); } /// Get value of parameter using c++ types as template argument. template - decltype(auto) - get_value() const; + decltype(auto) get_value() const; RCLCPP_PUBLIC - bool - as_bool() const; + bool as_bool() const; RCLCPP_PUBLIC - int64_t - as_int() const; + int64_t as_int() const; RCLCPP_PUBLIC - double - as_double() const; + double as_double() const; RCLCPP_PUBLIC - const std::string & - as_string() const; + const std::string & as_string() const; RCLCPP_PUBLIC - const std::vector & - as_byte_array() const; + const std::vector & as_byte_array() const; RCLCPP_PUBLIC - const std::vector & - as_bool_array() const; + const std::vector & as_bool_array() const; RCLCPP_PUBLIC - const std::vector & - as_integer_array() const; + const std::vector & as_integer_array() const; RCLCPP_PUBLIC - const std::vector & - as_double_array() const; + const std::vector & as_double_array() const; RCLCPP_PUBLIC - const std::vector & - as_string_array() const; + const std::vector & as_string_array() const; RCLCPP_PUBLIC - static Parameter - from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter); + static Parameter from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter); RCLCPP_PUBLIC - rcl_interfaces::msg::Parameter - to_parameter_msg() const; + rcl_interfaces::msg::Parameter to_parameter_msg() const; RCLCPP_PUBLIC - std::string - value_to_string() const; + std::string value_to_string() const; private: std::string name_; @@ -171,55 +149,43 @@ class Parameter /// Return a json encoded version of the parameter intended for a dict. RCLCPP_PUBLIC -std::string -_to_json_dict_entry(const Parameter & param); +std::string _to_json_dict_entry(const Parameter & param); RCLCPP_PUBLIC -std::ostream & -operator<<(std::ostream & os, const rclcpp::Parameter & pv); +std::ostream & operator<<(std::ostream & os, const rclcpp::Parameter & pv); RCLCPP_PUBLIC -std::ostream & -operator<<(std::ostream & os, const std::vector & parameters); +std::ostream & operator<<(std::ostream & os, const std::vector & parameters); namespace detail { template -auto -get_value_helper(const rclcpp::Parameter * parameter) +auto get_value_helper(const rclcpp::Parameter * parameter) { return parameter->get_parameter_value().get(); } // Specialization allowing Parameter::get() to return a const ref to the parameter value object. template<> -inline -auto -get_value_helper(const rclcpp::Parameter * parameter) +inline auto get_value_helper(const rclcpp::Parameter * parameter) { return parameter->get_parameter_value(); } // Specialization allowing Parameter::get() to return a const ref to the parameter itself. template<> -inline -auto -get_value_helper(const rclcpp::Parameter * parameter) +inline auto get_value_helper(const rclcpp::Parameter * parameter) { // Use this lambda to ensure it's a const reference being returned (and not a copy). - auto type_enforcing_lambda = - [¶meter]() -> const rclcpp::Parameter & { - return *parameter; - }; + auto type_enforcing_lambda = [¶meter]() -> const rclcpp::Parameter & { return *parameter; }; return type_enforcing_lambda(); } } // namespace detail template -decltype(auto) -Parameter::get_value() const +decltype(auto) Parameter::get_value() const { // use the helper to specialize for the ParameterValue and Parameter cases. return detail::get_value_helper(this); @@ -232,13 +198,11 @@ namespace std /// Return a json encoded version of the parameter intended for a list. RCLCPP_PUBLIC -std::string -to_string(const rclcpp::Parameter & param); +std::string to_string(const rclcpp::Parameter & param); /// Return a json encoded version of a vector of parameters, as a string. RCLCPP_PUBLIC -std::string -to_string(const std::vector & parameters); +std::string to_string(const std::vector & parameters); } // namespace std diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 2cf09d8ae4..ab45ab54b2 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -29,8 +29,8 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "rcl_interfaces/srv/set_parameters.hpp" #include "rcl_interfaces/srv/set_parameters_atomically.hpp" -#include "rclcpp/executors.hpp" #include "rclcpp/create_subscription.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" @@ -71,64 +71,43 @@ class AsyncParametersClient rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); RCLCPP_PUBLIC - std::shared_future> - get_parameters( + std::shared_future> get_parameters( const std::vector & names, - std::function< - void(std::shared_future>) - > callback = nullptr); + std::function>)> callback = nullptr); RCLCPP_PUBLIC - std::shared_future> - get_parameter_types( + std::shared_future> get_parameter_types( const std::vector & names, - std::function< - void(std::shared_future>) - > callback = nullptr); + std::function>)> callback = nullptr); RCLCPP_PUBLIC - std::shared_future> - set_parameters( + std::shared_future> set_parameters( const std::vector & parameters, - std::function< - void(std::shared_future>) - > callback = nullptr); + std::function>)> + callback = nullptr); RCLCPP_PUBLIC - std::shared_future - set_parameters_atomically( + std::shared_future set_parameters_atomically( const std::vector & parameters, - std::function< - void(std::shared_future) - > callback = nullptr); + std::function)> callback = + nullptr); RCLCPP_PUBLIC - std::shared_future - list_parameters( + std::shared_future list_parameters( const std::vector & prefixes, uint64_t depth, - std::function< - void(std::shared_future) - > callback = nullptr); - - template< - typename CallbackT, - typename AllocatorT = std::allocator> - typename rclcpp::Subscription::SharedPtr - on_parameter_event( + std::function)> callback = + nullptr); + + template> + typename rclcpp::Subscription::SharedPtr on_parameter_event( CallbackT && callback, - const rclcpp::QoS & qos = ( - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)) - ), - const rclcpp::SubscriptionOptionsWithAllocator & options = ( - rclcpp::SubscriptionOptionsWithAllocator() - )) + const rclcpp::QoS & qos = + (rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))), + const rclcpp::SubscriptionOptionsWithAllocator & options = + (rclcpp::SubscriptionOptionsWithAllocator())) { - return this->on_parameter_event( - this->node_topics_interface_, - callback, - qos, - options); + return this->on_parameter_event(this->node_topics_interface_, callback, qos, options); } /** @@ -136,59 +115,44 @@ class AsyncParametersClient * which returns a shared_ptr to a NodeTopicsInterface, or be a * NodeTopicsInterface pointer itself. */ - template< - typename CallbackT, - typename NodeT, - typename AllocatorT = std::allocator> + template> static typename rclcpp::Subscription::SharedPtr on_parameter_event( NodeT && node, CallbackT && callback, - const rclcpp::QoS & qos = ( - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)) - ), - const rclcpp::SubscriptionOptionsWithAllocator & options = ( - rclcpp::SubscriptionOptionsWithAllocator() - )) + const rclcpp::QoS & qos = + (rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))), + const rclcpp::SubscriptionOptionsWithAllocator & options = + (rclcpp::SubscriptionOptionsWithAllocator())) { return rclcpp::create_subscription( - node, - "parameter_events", - qos, - std::forward(callback), - options); + node, "parameter_events", qos, std::forward(callback), options); } RCLCPP_PUBLIC - bool - service_is_ready() const; + bool service_is_ready() const; template - bool - wait_for_service( + bool wait_for_service( std::chrono::duration timeout = std::chrono::duration(-1)) { return wait_for_service_nanoseconds( - std::chrono::duration_cast(timeout) - ); + std::chrono::duration_cast(timeout)); } protected: RCLCPP_PUBLIC - bool - wait_for_service_nanoseconds(std::chrono::nanoseconds timeout); + bool wait_for_service_nanoseconds(std::chrono::nanoseconds timeout); private: rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_; rclcpp::Client::SharedPtr get_parameters_client_; - rclcpp::Client::SharedPtr - get_parameter_types_client_; + rclcpp::Client::SharedPtr get_parameter_types_client_; rclcpp::Client::SharedPtr set_parameters_client_; rclcpp::Client::SharedPtr set_parameters_atomically_client_; rclcpp::Client::SharedPtr list_parameters_client_; - rclcpp::Client::SharedPtr - describe_parameters_client_; + rclcpp::Client::SharedPtr describe_parameters_client_; std::string remote_node_name_; }; @@ -234,16 +198,13 @@ class SyncParametersClient const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); RCLCPP_PUBLIC - std::vector - get_parameters(const std::vector & parameter_names); + std::vector get_parameters(const std::vector & parameter_names); RCLCPP_PUBLIC - bool - has_parameter(const std::string & parameter_name); + bool has_parameter(const std::string & parameter_name); template - T - get_parameter_impl( + T get_parameter_impl( const std::string & parameter_name, std::function parameter_not_found_handler) { std::vector names; @@ -257,51 +218,42 @@ class SyncParametersClient } template - T - get_parameter(const std::string & parameter_name, const T & default_value) + T get_parameter(const std::string & parameter_name, const T & default_value) { return get_parameter_impl( - parameter_name, - std::function([&default_value]() -> T {return default_value;})); + parameter_name, std::function([&default_value]() -> T { return default_value; })); } template - T - get_parameter(const std::string & parameter_name) + T get_parameter(const std::string & parameter_name) { return get_parameter_impl( - parameter_name, - std::function([¶meter_name]() -> T - { + parameter_name, std::function([¶meter_name]() -> T { throw std::runtime_error("Parameter '" + parameter_name + "' is not set"); - }) - ); + })); } RCLCPP_PUBLIC - std::vector - get_parameter_types(const std::vector & parameter_names); + std::vector get_parameter_types( + const std::vector & parameter_names); RCLCPP_PUBLIC - std::vector - set_parameters(const std::vector & parameters); + std::vector set_parameters( + const std::vector & parameters); RCLCPP_PUBLIC - rcl_interfaces::msg::SetParametersResult - set_parameters_atomically(const std::vector & parameters); + rcl_interfaces::msg::SetParametersResult set_parameters_atomically( + const std::vector & parameters); RCLCPP_PUBLIC - rcl_interfaces::msg::ListParametersResult - list_parameters( - const std::vector & parameter_prefixes, - uint64_t depth); + rcl_interfaces::msg::ListParametersResult list_parameters( + const std::vector & parameter_prefixes, uint64_t depth); template - typename rclcpp::Subscription::SharedPtr - on_parameter_event(CallbackT && callback) + typename rclcpp::Subscription::SharedPtr on_parameter_event( + CallbackT && callback) { - return async_parameters_client_->on_parameter_event( - std::forward(callback)); + return async_parameters_client_->on_parameter_event(std::forward(callback)); } /** @@ -309,29 +261,21 @@ class SyncParametersClient * which returns a shared_ptr to a NodeTopicsInterface, or be a * NodeTopicsInterface pointer itself. */ - template< - typename CallbackT, - typename NodeT> + template static typename rclcpp::Subscription::SharedPtr - on_parameter_event( - NodeT && node, - CallbackT && callback) + on_parameter_event(NodeT && node, CallbackT && callback) { - return AsyncParametersClient::on_parameter_event( - node, - std::forward(callback)); + return AsyncParametersClient::on_parameter_event(node, std::forward(callback)); } RCLCPP_PUBLIC - bool - service_is_ready() const + bool service_is_ready() const { return async_parameters_client_->service_is_ready(); } template - bool - wait_for_service( + bool wait_for_service( std::chrono::duration timeout = std::chrono::duration(-1)) { return async_parameters_client_->wait_for_service(timeout); diff --git a/rclcpp/include/rclcpp/parameter_events_filter.hpp b/rclcpp/include/rclcpp/parameter_events_filter.hpp index d09d287762..2d459655a5 100644 --- a/rclcpp/include/rclcpp/parameter_events_filter.hpp +++ b/rclcpp/include/rclcpp/parameter_events_filter.hpp @@ -35,7 +35,7 @@ class ParameterEventsFilter { public: RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsFilter) - enum class EventType {NEW, DELETED, CHANGED}; ///< An enum for the type of event. + enum class EventType { NEW, DELETED, CHANGED }; ///< An enum for the type of event. /// Used for the listed results using EventPair = std::pair; @@ -73,7 +73,7 @@ class ParameterEventsFilter private: // access only allowed via const accessor. - std::vector result_; ///< Storage of the resultant vector + std::vector result_; ///< Storage of the resultant vector rcl_interfaces::msg::ParameterEvent::SharedPtr event_; ///< Keep event in scope }; diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 09b59776a6..41e5782fb4 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -37,16 +37,14 @@ using ParameterMap = std::unordered_map>; /// \returns a map where the keys are fully qualified node names and values a list of parameters. /// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid. RCLCPP_PUBLIC -ParameterMap -parameter_map_from(const rcl_params_t * const c_params); +ParameterMap parameter_map_from(const rcl_params_t * const c_params); /// Convert parameter value from rcl_yaml_param_parser into a C++ class instance. /// \param[in] c_value C structure containing a value of a parameter. /// \returns an instance of a parameter value /// \throws InvalidParameterValueException if the `rcl_variant_t` is inconsistent or invalid. RCLCPP_PUBLIC -ParameterValue -parameter_value_from(const rcl_variant_t * const c_value); +ParameterValue parameter_value_from(const rcl_variant_t * const c_value); } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index 143a5223c8..a88c637536 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -48,13 +48,11 @@ class ParameterService private: rclcpp::Service::SharedPtr get_parameters_service_; - rclcpp::Service::SharedPtr - get_parameter_types_service_; + rclcpp::Service::SharedPtr get_parameter_types_service_; rclcpp::Service::SharedPtr set_parameters_service_; rclcpp::Service::SharedPtr set_parameters_atomically_service_; - rclcpp::Service::SharedPtr - describe_parameters_service_; + rclcpp::Service::SharedPtr describe_parameters_service_; rclcpp::Service::SharedPtr list_parameters_service_; }; diff --git a/rclcpp/include/rclcpp/parameter_value.hpp b/rclcpp/include/rclcpp/parameter_value.hpp index 77bcdec51f..caa0f521e4 100644 --- a/rclcpp/include/rclcpp/parameter_value.hpp +++ b/rclcpp/include/rclcpp/parameter_value.hpp @@ -29,8 +29,7 @@ namespace rclcpp { -enum ParameterType : uint8_t -{ +enum ParameterType : uint8_t { PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET, PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, PARAMETER_INTEGER = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, @@ -45,12 +44,10 @@ enum ParameterType : uint8_t /// Return the name of a parameter type RCLCPP_PUBLIC -std::string -to_string(ParameterType type); +std::string to_string(ParameterType type); RCLCPP_PUBLIC -std::ostream & -operator<<(std::ostream & os, ParameterType type); +std::ostream & operator<<(std::ostream & os, ParameterType type); /// Indicate the parameter type does not match the expected type. class ParameterTypeException : public std::runtime_error @@ -64,7 +61,8 @@ class ParameterTypeException : public std::runtime_error RCLCPP_PUBLIC ParameterTypeException(ParameterType expected, ParameterType actual) : std::runtime_error("expected [" + to_string(expected) + "] got [" + to_string(actual) + "]") - {} + { + } }; /// Store the type and value of a parameter. @@ -122,30 +120,25 @@ class ParameterValue /// Return an enum indicating the type of the set value. RCLCPP_PUBLIC - ParameterType - get_type() const; + ParameterType get_type() const; /// Return a message populated with the parameter value RCLCPP_PUBLIC - rcl_interfaces::msg::ParameterValue - to_value_msg() const; + rcl_interfaces::msg::ParameterValue to_value_msg() const; /// Equal operator. RCLCPP_PUBLIC - bool - operator==(const ParameterValue & rhs) const; + bool operator==(const ParameterValue & rhs) const; /// Not equal operator. RCLCPP_PUBLIC - bool - operator!=(const ParameterValue & rhs) const; + bool operator!=(const ParameterValue & rhs) const; // The following get() variants require the use of ParameterType template - constexpr - typename std::enable_if::type - get() const + constexpr typename std::enable_if::type get() + const { if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { throw ParameterTypeException(ParameterType::PARAMETER_BOOL, get_type()); @@ -154,8 +147,7 @@ class ParameterValue } template - constexpr - typename std::enable_if::type + constexpr typename std::enable_if::type get() const { if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) { @@ -165,8 +157,7 @@ class ParameterValue } template - constexpr - typename std::enable_if::type + constexpr typename std::enable_if::type get() const { if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { @@ -177,8 +168,8 @@ class ParameterValue template constexpr - typename std::enable_if::type - get() const + typename std::enable_if::type + get() const { if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) { throw ParameterTypeException(ParameterType::PARAMETER_STRING, get_type()); @@ -187,10 +178,9 @@ class ParameterValue } template - constexpr - typename std::enable_if< - type == ParameterType::PARAMETER_BYTE_ARRAY, const std::vector &>::type - get() const + constexpr typename std:: + enable_if &>::type + get() const { if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY) { throw ParameterTypeException(ParameterType::PARAMETER_BYTE_ARRAY, get_type()); @@ -199,10 +189,9 @@ class ParameterValue } template - constexpr - typename std::enable_if< - type == ParameterType::PARAMETER_BOOL_ARRAY, const std::vector &>::type - get() const + constexpr typename std:: + enable_if &>::type + get() const { if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY) { throw ParameterTypeException(ParameterType::PARAMETER_BOOL_ARRAY, get_type()); @@ -211,10 +200,9 @@ class ParameterValue } template - constexpr - typename std::enable_if< - type == ParameterType::PARAMETER_INTEGER_ARRAY, const std::vector &>::type - get() const + constexpr typename std:: + enable_if &>::type + get() const { if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY) { throw ParameterTypeException(ParameterType::PARAMETER_INTEGER_ARRAY, get_type()); @@ -223,10 +211,9 @@ class ParameterValue } template - constexpr - typename std::enable_if< - type == ParameterType::PARAMETER_DOUBLE_ARRAY, const std::vector &>::type - get() const + constexpr typename std:: + enable_if &>::type + get() const { if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY) { throw ParameterTypeException(ParameterType::PARAMETER_DOUBLE_ARRAY, get_type()); @@ -235,10 +222,9 @@ class ParameterValue } template - constexpr - typename std::enable_if< - type == ParameterType::PARAMETER_STRING_ARRAY, const std::vector &>::type - get() const + constexpr typename std:: + enable_if &>::type + get() const { if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY) { throw ParameterTypeException(ParameterType::PARAMETER_STRING_ARRAY, get_type()); @@ -249,83 +235,75 @@ class ParameterValue // The following get() variants allow the use of primitive types template - constexpr - typename std::enable_if::value, const bool &>::type - get() const + constexpr typename std::enable_if::value, const bool &>::type get() const { return get(); } template - constexpr - typename std::enable_if< - std::is_integral::value && !std::is_same::value, const int64_t &>::type + constexpr typename std::enable_if< + std::is_integral::value && !std::is_same::value, + const int64_t &>::type get() const { return get(); } template - constexpr - typename std::enable_if::value, const double &>::type - get() const + constexpr typename std::enable_if::value, const double &>::type get() + const { return get(); } template - constexpr - typename std::enable_if::value, const std::string &>::type - get() const + constexpr typename std:: + enable_if::value, const std::string &>::type + get() const { return get(); } template - constexpr - typename std::enable_if< - std::is_convertible< - type, const std::vector &>::value, const std::vector &>::type + constexpr typename std::enable_if< + std::is_convertible &>::value, + const std::vector &>::type get() const { return get(); } template - constexpr - typename std::enable_if< - std::is_convertible< - type, const std::vector &>::value, const std::vector &>::type + constexpr typename std::enable_if< + std::is_convertible &>::value, + const std::vector &>::type get() const { return get(); } template - constexpr - typename std::enable_if< - std::is_convertible< - type, const std::vector &>::value, const std::vector &>::type + constexpr typename std::enable_if< + std::is_convertible &>::value, + const std::vector &>::type get() const { return get(); } template - constexpr - typename std::enable_if< - std::is_convertible< - type, const std::vector &>::value, const std::vector &>::type + constexpr typename std::enable_if< + std::is_convertible &>::value, + const std::vector &>::type get() const { return get(); } template - constexpr - typename std::enable_if< - std::is_convertible< - type, const std::vector &>::value, const std::vector &>::type + constexpr typename std::enable_if< + std::is_convertible &>::value, + const std::vector &>::type get() const { return get(); @@ -337,8 +315,7 @@ class ParameterValue /// Return the value of a parameter as a string RCLCPP_PUBLIC -std::string -to_string(const ParameterValue & type); +std::string to_string(const ParameterValue & type); } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 3357d54646..f556625f7a 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -71,8 +71,7 @@ class Publisher : public PublisherBase if (event_callbacks.deadline_callback) { this->add_event_handler( - event_callbacks.deadline_callback, - RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + event_callbacks.deadline_callback, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); } if (event_callbacks.liveliness_callback) { this->add_event_handler(event_callbacks.liveliness_callback, RCL_PUBLISHER_LIVELINESS_LOST); @@ -80,15 +79,15 @@ class Publisher : public PublisherBase } virtual ~Publisher() - {} + { + } - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - make_mapped_ring_buffer(size_t size) const override + mapped_ring_buffer::MappedRingBufferBase::SharedPtr make_mapped_ring_buffer( + size_t size) const override { - return mapped_ring_buffer::MappedRingBuffer< - MessageT, - typename Publisher::MessageAlloc - >::make_shared(size, this->get_allocator()); + return mapped_ring_buffer:: + MappedRingBuffer::MessageAlloc>::make_shared( + size, this->get_allocator()); } /// Send a message to the topic for this publisher. @@ -96,8 +95,7 @@ class Publisher : public PublisherBase * This function is templated on the input message type, MessageT. * \param[in] msg A shared pointer to the message to send. */ - virtual void - publish(std::unique_ptr msg) + virtual void publish(std::unique_ptr msg) { if (!intra_process_is_enabled_) { this->do_inter_process_publish(msg.get()); @@ -115,11 +113,9 @@ class Publisher : public PublisherBase MessageSharedPtr shared_msg; if (inter_process_publish_needed) { shared_msg = std::move(msg); - message_seq = - store_intra_process_message(intra_process_publisher_id_, shared_msg); + message_seq = store_intra_process_message(intra_process_publisher_id_, shared_msg); } else { - message_seq = - store_intra_process_message(intra_process_publisher_id_, std::move(msg)); + message_seq = store_intra_process_message(intra_process_publisher_id_, std::move(msg)); } this->do_intra_process_publish(message_seq); if (inter_process_publish_needed) { @@ -127,8 +123,7 @@ class Publisher : public PublisherBase } } - virtual void - publish(const MessageT & msg) + virtual void publish(const MessageT & msg) { // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { @@ -144,8 +139,7 @@ class Publisher : public PublisherBase this->publish(std::move(unique_msg)); } - void - publish(const rcl_serialized_message_t & serialized_msg) + void publish(const rcl_serialized_message_t & serialized_msg) { return this->do_serialized_publish(&serialized_msg); } @@ -156,8 +150,7 @@ class Publisher : public PublisherBase } protected: - void - do_inter_process_publish(const MessageT * msg) + void do_inter_process_publish(const MessageT * msg) { auto status = rcl_publish(&publisher_handle_, msg, nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { @@ -175,8 +168,7 @@ class Publisher : public PublisherBase } } - void - do_serialized_publish(const rcl_serialized_message_t * serialized_msg) + void do_serialized_publish(const rcl_serialized_message_t * serialized_msg) { if (intra_process_is_enabled_) { // TODO(Karsten1987): support serialized message passed by intraprocess @@ -188,8 +180,7 @@ class Publisher : public PublisherBase } } - void - do_intra_process_publish(uint64_t message_seq) + void do_intra_process_publish(uint64_t message_seq) { rcl_interfaces::msg::IntraProcessMessage ipm; ipm.publisher_id = intra_process_publisher_id_; @@ -210,15 +201,12 @@ class Publisher : public PublisherBase } } - uint64_t - store_intra_process_message( - uint64_t publisher_id, - std::shared_ptr msg) + uint64_t store_intra_process_message(uint64_t publisher_id, std::shared_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { throw std::runtime_error( - "intra process publish called after destruction of intra process manager"); + "intra process publish called after destruction of intra process manager"); } if (!msg) { throw std::runtime_error("cannot publisher msg which is a null pointer"); @@ -228,15 +216,13 @@ class Publisher : public PublisherBase return message_seq; } - uint64_t - store_intra_process_message( - uint64_t publisher_id, - std::unique_ptr msg) + uint64_t store_intra_process_message( + uint64_t publisher_id, std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { throw std::runtime_error( - "intra process publish called after destruction of intra process manager"); + "intra process publish called after destruction of intra process manager"); } if (!msg) { throw std::runtime_error("cannot publisher msg which is a null pointer"); diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index afb68302f8..46e1e7aeb1 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -41,7 +41,7 @@ namespace node_interfaces { class NodeBaseInterface; class NodeTopicsInterface; -} +} // namespace node_interfaces namespace intra_process_manager { @@ -50,7 +50,7 @@ namespace intra_process_manager * `intra_process_manager.hpp` and `publisher_base.hpp`. */ class IntraProcessManager; -} +} // namespace intra_process_manager class PublisherBase { @@ -81,56 +81,47 @@ class PublisherBase /// Get the topic that this publisher publishes on. /** \return The topic name. */ RCLCPP_PUBLIC - const char * - get_topic_name() const; + const char * get_topic_name() const; /// Get the queue size for this publisher. /** \return The queue size. */ RCLCPP_PUBLIC - size_t - get_queue_size() const; + size_t get_queue_size() const; /// Get the global identifier for this publisher (used in rmw and by DDS). /** \return The gid. */ RCLCPP_PUBLIC - const rmw_gid_t & - get_gid() const; + const rmw_gid_t & get_gid() const; /// Get the global identifier for this publisher used by intra-process communication. /** \return The intra-process gid. */ RCLCPP_PUBLIC - const rmw_gid_t & - get_intra_process_gid() const; + const rmw_gid_t & get_intra_process_gid() const; /// Get the rcl publisher handle. /** \return The rcl publisher handle. */ RCLCPP_PUBLIC - rcl_publisher_t * - get_publisher_handle(); + rcl_publisher_t * get_publisher_handle(); /// Get the rcl publisher handle. /** \return The rcl publisher handle. */ RCLCPP_PUBLIC - const rcl_publisher_t * - get_publisher_handle() const; + const rcl_publisher_t * get_publisher_handle() const; /// Get all the QoS event handlers associated with this publisher. /** \return The vector of QoS event handlers. */ RCLCPP_PUBLIC - const std::vector> & - get_event_handlers() const; + const std::vector> & get_event_handlers() const; /// Get subscription count /** \return The number of subscriptions. */ RCLCPP_PUBLIC - size_t - get_subscription_count() const; + size_t get_subscription_count() const; /// Get intraprocess subscription count /** \return The number of intraprocess subscriptions. */ RCLCPP_PUBLIC - size_t - get_intra_process_subscription_count() const; + size_t get_intra_process_subscription_count() const; /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC). /** @@ -142,8 +133,7 @@ class PublisherBase */ RCLCPP_PUBLIC RCUTILS_WARN_UNUSED - bool - assert_liveliness() const; + bool assert_liveliness() const; /// Get the actual QoS settings, after the defaults have been determined. /** @@ -156,8 +146,7 @@ class PublisherBase * \return The actual qos settings. */ RCLCPP_PUBLIC - rmw_qos_profile_t - get_actual_qos() const; + rmw_qos_profile_t get_actual_qos() const; /// Compare this publisher to a gid. /** @@ -166,8 +155,7 @@ class PublisherBase * \return True if the publisher's gid matches the input. */ RCLCPP_PUBLIC - bool - operator==(const rmw_gid_t & gid) const; + bool operator==(const rmw_gid_t & gid) const; /// Compare this publisher to a pointer gid. /** @@ -176,37 +164,30 @@ class PublisherBase * \return True if this publisher's gid matches the input. */ RCLCPP_PUBLIC - bool - operator==(const rmw_gid_t * gid) const; + bool operator==(const rmw_gid_t * gid) const; using IntraProcessManagerSharedPtr = std::shared_ptr; /// Implementation utility function that creates a typed mapped ring buffer. RCLCPP_PUBLIC - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - virtual make_mapped_ring_buffer(size_t size) const; + mapped_ring_buffer::MappedRingBufferBase::SharedPtr virtual make_mapped_ring_buffer( + size_t size) const; /// Implementation utility function used to setup intra process publishing after creation. RCLCPP_PUBLIC - void - setup_intra_process( + void setup_intra_process( uint64_t intra_process_publisher_id, IntraProcessManagerSharedPtr ipm, const rcl_publisher_options_t & intra_process_options); protected: template - void - add_event_handler( - const EventCallbackT & callback, - const rcl_publisher_event_type_t event_type) + void add_event_handler( + const EventCallbackT & callback, const rcl_publisher_event_type_t event_type) { auto handler = std::make_shared>( - callback, - rcl_publisher_event_init, - &publisher_handle_, - event_type); + callback, rcl_publisher_event_init, &publisher_handle_, event_type); event_handlers_.emplace_back(handler); } diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index 8e6e8e27fb..b74ee0ab4c 100644 --- a/rclcpp/include/rclcpp/publisher_factory.hpp +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -23,9 +23,9 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" -#include "rclcpp/publisher.hpp" #include "rclcpp/intra_process_manager.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/publisher.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -45,43 +45,34 @@ namespace rclcpp struct PublisherFactory { // Creates a PublisherT publisher object and returns it as a PublisherBase. - using PublisherFactoryFunction = std::function< - rclcpp::PublisherBase::SharedPtr( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const std::string & topic_name, - const rcl_publisher_options_t & publisher_options)>; + using PublisherFactoryFunction = std::function; PublisherFactoryFunction create_typed_publisher; }; /// Return a PublisherFactory with functions setup for creating a PublisherT. template -PublisherFactory -create_publisher_factory( - const PublisherEventCallbacks & event_callbacks, - std::shared_ptr allocator) +PublisherFactory create_publisher_factory( + const PublisherEventCallbacks & event_callbacks, std::shared_ptr allocator) { PublisherFactory factory; // factory function that creates a MessageT specific PublisherT factory.create_typed_publisher = [event_callbacks, allocator]( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const std::string & topic_name, - const rcl_publisher_options_t & publisher_options - ) -> std::shared_ptr - { - auto options_copy = publisher_options; - auto message_alloc = std::make_shared(*allocator.get()); - options_copy.allocator = allocator::get_rcl_allocator(*message_alloc.get()); + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + const rcl_publisher_options_t & publisher_options) -> std::shared_ptr { + auto options_copy = publisher_options; + auto message_alloc = std::make_shared(*allocator.get()); + options_copy.allocator = allocator::get_rcl_allocator(*message_alloc.get()); - return std::make_shared( - node_base, - topic_name, - options_copy, - event_callbacks, - message_alloc); - }; + return std::make_shared( + node_base, topic_name, options_copy, event_callbacks, message_alloc); + }; // return the factory now that it is populated return factory; diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index d1a7f6455c..e002280fab 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -19,12 +19,12 @@ #include #include +#include "rcl/publisher.h" #include "rclcpp/callback_group.hpp" #include "rclcpp/intra_process_setting.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/visibility_control.hpp" -#include "rcl/publisher.h" namespace rclcpp { @@ -49,17 +49,19 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase /// Optional custom allocator. std::shared_ptr allocator = nullptr; - PublisherOptionsWithAllocator() {} + PublisherOptionsWithAllocator() + { + } /// Constructor using base class as input. explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base) : PublisherOptionsBase(publisher_options_base) - {} + { + } /// Convert this class, and a rclcpp::QoS, into an rcl_publisher_options_t. template - rcl_publisher_options_t - to_rcl_publisher_options(const rclcpp::QoS & qos) const + rcl_publisher_options_t to_rcl_publisher_options(const rclcpp::QoS & qos) const { rcl_publisher_options_t result; using AllocatorTraits = std::allocator_traits; diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index 6f7db7894e..a6e4bd2af5 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -34,9 +34,7 @@ struct RCLCPP_PUBLIC QoSInitialization QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg); /// Create a QoSInitialization from an existing rmw_qos_profile_t, using its history and depth. - static - QoSInitialization - from_rmw(const rmw_qos_profile_t & rmw_qos); + static QoSInitialization from_rmw(const rmw_qos_profile_t & rmw_qos); }; /// Use to initialize the QoS with the keep_all history setting. @@ -56,8 +54,7 @@ class RCLCPP_PUBLIC QoS { public: /// Constructor which allows you to construct a QoS by giving the only required settings. - explicit - QoS( + explicit QoS( const QoSInitialization & qos_initialization, const rmw_qos_profile_t & initial_profile = rmw_qos_profile_default); @@ -69,83 +66,64 @@ class RCLCPP_PUBLIC QoS QoS(size_t history_depth); // NOLINT(runtime/explicit): conversion constructor /// Return the rmw qos profile. - rmw_qos_profile_t & - get_rmw_qos_profile(); + rmw_qos_profile_t & get_rmw_qos_profile(); /// Return the rmw qos profile. - const rmw_qos_profile_t & - get_rmw_qos_profile() const; + const rmw_qos_profile_t & get_rmw_qos_profile() const; /// Set the history policy. - QoS & - history(rmw_qos_history_policy_t history); + QoS & history(rmw_qos_history_policy_t history); /// Set the history to keep last. - QoS & - keep_last(size_t depth); + QoS & keep_last(size_t depth); /// Set the history to keep all. - QoS & - keep_all(); + QoS & keep_all(); /// Set the reliability setting. - QoS & - reliability(rmw_qos_reliability_policy_t reliability); + QoS & reliability(rmw_qos_reliability_policy_t reliability); /// Set the reliability setting to reliable. - QoS & - reliable(); + QoS & reliable(); /// Set the reliability setting to best effort. - QoS & - best_effort(); + QoS & best_effort(); /// Set the durability setting. - QoS & - durability(rmw_qos_durability_policy_t durability); + QoS & durability(rmw_qos_durability_policy_t durability); /// Set the durability setting to volatile. /** * Note that this cannot be named `volatile` because it is a C++ keyword. */ - QoS & - durability_volatile(); + QoS & durability_volatile(); /// Set the durability setting to transient local. - QoS & - transient_local(); + QoS & transient_local(); /// Set the deadline setting. - QoS & - deadline(rmw_time_t deadline); + QoS & deadline(rmw_time_t deadline); /// Set the deadline setting, rclcpp::Duration. - QoS & - deadline(const rclcpp::Duration & deadline); + QoS & deadline(const rclcpp::Duration & deadline); /// Set the lifespan setting. - QoS & - lifespan(rmw_time_t lifespan); + QoS & lifespan(rmw_time_t lifespan); /// Set the lifespan setting, rclcpp::Duration. - QoS & - lifespan(const rclcpp::Duration & lifespan); + QoS & lifespan(const rclcpp::Duration & lifespan); /// Set the liveliness setting. - QoS & - liveliness(rmw_qos_liveliness_policy_t liveliness); + QoS & liveliness(rmw_qos_liveliness_policy_t liveliness); /// Set the liveliness_lease_duration setting. - QoS & - liveliness_lease_duration(rmw_time_t liveliness_lease_duration); + QoS & liveliness_lease_duration(rmw_time_t liveliness_lease_duration); /// Set the liveliness_lease_duration setting, rclcpp::Duration. - QoS & - liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration); + QoS & liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration); /// Set the avoid_ros_namespace_conventions setting. - QoS & - avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions); + QoS & avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions); private: rmw_qos_profile_t rmw_qos_profile_; @@ -154,51 +132,41 @@ class RCLCPP_PUBLIC QoS class RCLCPP_PUBLIC SensorDataQoS : public QoS { public: - explicit - SensorDataQoS( - const QoSInitialization & qos_initialization = ( - QoSInitialization::from_rmw(rmw_qos_profile_sensor_data) - )); + explicit SensorDataQoS( + const QoSInitialization & qos_initialization = + (QoSInitialization::from_rmw(rmw_qos_profile_sensor_data))); }; class RCLCPP_PUBLIC ParametersQoS : public QoS { public: - explicit - ParametersQoS( - const QoSInitialization & qos_initialization = ( - QoSInitialization::from_rmw(rmw_qos_profile_parameters) - )); + explicit ParametersQoS( + const QoSInitialization & qos_initialization = + (QoSInitialization::from_rmw(rmw_qos_profile_parameters))); }; class RCLCPP_PUBLIC ServicesQoS : public QoS { public: - explicit - ServicesQoS( - const QoSInitialization & qos_initialization = ( - QoSInitialization::from_rmw(rmw_qos_profile_services_default) - )); + explicit ServicesQoS( + const QoSInitialization & qos_initialization = + (QoSInitialization::from_rmw(rmw_qos_profile_services_default))); }; class RCLCPP_PUBLIC ParameterEventsQoS : public QoS { public: - explicit - ParameterEventsQoS( - const QoSInitialization & qos_initialization = ( - QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) - )); + explicit ParameterEventsQoS( + const QoSInitialization & qos_initialization = + (QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))); }; class RCLCPP_PUBLIC SystemDefaultsQoS : public QoS { public: - explicit - SystemDefaultsQoS( - const QoSInitialization & qos_initialization = ( - QoSInitialization::from_rmw(rmw_qos_profile_system_default) - )); + explicit SystemDefaultsQoS( + const QoSInitialization & qos_initialization = + (QoSInitialization::from_rmw(rmw_qos_profile_system_default))); }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index d14af2bbe9..def9274d51 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -33,10 +33,10 @@ using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t; using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t; using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t; -using QOSDeadlineRequestedCallbackType = std::function; -using QOSDeadlineOfferedCallbackType = std::function; -using QOSLivelinessChangedCallbackType = std::function; -using QOSLivelinessLostCallbackType = std::function; +using QOSDeadlineRequestedCallbackType = std::function; +using QOSDeadlineOfferedCallbackType = std::function; +using QOSLivelinessChangedCallbackType = std::function; +using QOSLivelinessLostCallbackType = std::function; /// Contains callbacks for various types of events a Publisher can receive from the middleware. struct PublisherEventCallbacks @@ -60,18 +60,15 @@ class QOSEventHandlerBase : public Waitable /// Get the number of ready events RCLCPP_PUBLIC - size_t - get_number_of_ready_events() override; + size_t get_number_of_ready_events() override; /// Add the Waitable to a wait set. RCLCPP_PUBLIC - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; + bool add_to_wait_set(rcl_wait_set_t * wait_set) override; /// Check if the Waitable is ready. RCLCPP_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; + bool is_ready(rcl_wait_set_t * wait_set) override; protected: rcl_event_t event_handle_; @@ -98,16 +95,13 @@ class QOSEventHandler : public QOSEventHandlerBase } /// Execute any entities of the Waitable that are ready. - void - execute() override + void execute() override { EventCallbackInfoT callback_info; rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info); if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't take event info: %s", rcl_get_error_string().str); + RCUTILS_LOG_ERROR_NAMED("rclcpp", "Couldn't take event info: %s", rcl_get_error_string().str); return; } @@ -115,8 +109,9 @@ class QOSEventHandler : public QOSEventHandlerBase } private: - using EventCallbackInfoT = typename std::remove_reference::template argument_type<0>>::type; + using EventCallbackInfoT = + typename std::remove_reference::template argument_type<0>>::type; EventCallbackT event_callback_; }; diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 296cce14a1..a4eab4fb8f 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -47,15 +47,15 @@ class GenericRate : public RateBase RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) explicit GenericRate(double rate) - : GenericRate( - duration_cast(duration(1.0 / rate))) - {} + : GenericRate(duration_cast(duration(1.0 / rate))) + { + } explicit GenericRate(std::chrono::nanoseconds period) : period_(period), last_interval_(Clock::now()) - {} + { + } - virtual bool - sleep() + virtual bool sleep() { // Time coming into sleep auto now = Clock::now(); @@ -86,14 +86,12 @@ class GenericRate : public RateBase return true; } - virtual bool - is_steady() const + virtual bool is_steady() const { return Clock::is_steady; } - virtual void - reset() + virtual void reset() { last_interval_ = Clock::now(); } diff --git a/rclcpp/include/rclcpp/scope_exit.hpp b/rclcpp/include/rclcpp/scope_exit.hpp index 7865de5e48..86f4f8f7bd 100644 --- a/rclcpp/include/rclcpp/scope_exit.hpp +++ b/rclcpp/include/rclcpp/scope_exit.hpp @@ -29,17 +29,20 @@ namespace rclcpp template struct ScopeExit { - explicit ScopeExit(Callable callable) - : callable_(callable) {} - ~ScopeExit() {callable_();} + explicit ScopeExit(Callable callable) : callable_(callable) + { + } + ~ScopeExit() + { + callable_(); + } private: Callable callable_; }; template -ScopeExit -make_scope_exit(Callable callable) +ScopeExit make_scope_exit(Callable callable) { return ScopeExit(callable); } @@ -47,6 +50,6 @@ make_scope_exit(Callable callable) } // namespace rclcpp #define RCLCPP_SCOPE_EXIT(code) \ - auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code;}) + auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() { code; }) #endif // RCLCPP__SCOPE_EXIT_HPP_ diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9dc8d027b0..933c58bdb5 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -26,11 +26,11 @@ #include "rclcpp/any_service_callback.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/type_support_decl.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" -#include "rclcpp/logging.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -43,40 +43,33 @@ class ServiceBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase) RCLCPP_PUBLIC - explicit ServiceBase( - std::shared_ptr node_handle); + explicit ServiceBase(std::shared_ptr node_handle); RCLCPP_PUBLIC virtual ~ServiceBase(); RCLCPP_PUBLIC - const char * - get_service_name(); + const char * get_service_name(); RCLCPP_PUBLIC - std::shared_ptr - get_service_handle(); + std::shared_ptr get_service_handle(); RCLCPP_PUBLIC - std::shared_ptr - get_service_handle() const; + std::shared_ptr get_service_handle() const; 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; + std::shared_ptr request_header, std::shared_ptr request) = 0; protected: RCLCPP_DISABLE_COPY(ServiceBase) RCLCPP_PUBLIC - rcl_node_t * - get_rcl_node_handle(); + rcl_node_t * get_rcl_node_handle(); RCLCPP_PUBLIC - const rcl_node_t * - get_rcl_node_handle() const; + const rcl_node_t * get_rcl_node_handle() const; std::shared_ptr node_handle_; @@ -88,16 +81,14 @@ template class Service : public ServiceBase { public: - using CallbackType = std::function< - void ( - const std::shared_ptr, - std::shared_ptr)>; - - using CallbackWithHeaderType = std::function< - void ( - const std::shared_ptr, - const std::shared_ptr, - std::shared_ptr)>; + using CallbackType = std::function, + std::shared_ptr)>; + + using CallbackWithHeaderType = std::function, + const std::shared_ptr, + std::shared_ptr)>; RCLCPP_SMART_PTR_DEFINITIONS(Service) Service( @@ -113,8 +104,7 @@ class Service : public ServiceBase std::weak_ptr weak_node_handle(node_handle_); // rcl does the static memory allocation here service_handle_ = std::shared_ptr( - new rcl_service_t, [weak_node_handle](rcl_service_t * service) - { + new rcl_service_t, [weak_node_handle](rcl_service_t * service) { auto handle = weak_node_handle.lock(); if (handle) { if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) { @@ -160,8 +150,7 @@ class Service : public ServiceBase std::shared_ptr node_handle, std::shared_ptr service_handle, AnyServiceCallback any_callback) - : ServiceBase(node_handle), - any_callback_(any_callback) + : ServiceBase(node_handle), any_callback_(any_callback) { // check if service handle was initialized if (!rcl_service_is_valid(service_handle.get())) { @@ -178,8 +167,7 @@ class Service : public ServiceBase std::shared_ptr node_handle, rcl_service_t * service_handle, AnyServiceCallback any_callback) - : ServiceBase(node_handle), - any_callback_(any_callback) + : ServiceBase(node_handle), any_callback_(any_callback) { // check if service handle was initialized if (!rcl_service_is_valid(service_handle)) { @@ -213,8 +201,7 @@ class Service : public ServiceBase } void handle_request( - std::shared_ptr request_header, - std::shared_ptr request) + std::shared_ptr request_header, std::shared_ptr request) { auto typed_request = std::static_pointer_cast(request); auto response = std::shared_ptr(new typename ServiceT::Response); @@ -223,8 +210,7 @@ class Service : public ServiceBase } void send_response( - std::shared_ptr req_id, - std::shared_ptr response) + std::shared_ptr req_id, std::shared_ptr response) { rcl_ret_t status = rcl_send_response(get_service_handle().get(), req_id.get(), response.get()); diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 1ada13491e..73ff6ee2f6 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -126,28 +126,21 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy subscription_handles_.erase( std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr), - subscription_handles_.end() - ); + subscription_handles_.end()); service_handles_.erase( std::remove(service_handles_.begin(), service_handles_.end(), nullptr), - service_handles_.end() - ); + service_handles_.end()); client_handles_.erase( - std::remove(client_handles_.begin(), client_handles_.end(), nullptr), - client_handles_.end() - ); + std::remove(client_handles_.begin(), client_handles_.end(), nullptr), client_handles_.end()); timer_handles_.erase( - std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr), - timer_handles_.end() - ); + std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr), timer_handles_.end()); waitable_handles_.erase( std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr), - waitable_handles_.end() - ); + waitable_handles_.end()); } bool collect_entities(const WeakNodeList & weak_nodes) @@ -174,21 +167,21 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return false; }); group->find_service_ptrs_if([this](const rclcpp::ServiceBase::SharedPtr & service) { - service_handles_.push_back(service->get_service_handle()); - return false; - }); + service_handles_.push_back(service->get_service_handle()); + return false; + }); group->find_client_ptrs_if([this](const rclcpp::ClientBase::SharedPtr & client) { - client_handles_.push_back(client->get_client_handle()); - return false; - }); + client_handles_.push_back(client->get_client_handle()); + return false; + }); group->find_timer_ptrs_if([this](const rclcpp::TimerBase::SharedPtr & timer) { - timer_handles_.push_back(timer->get_timer_handle()); - return false; - }); + timer_handles_.push_back(timer->get_timer_handle()); + return false; + }); group->find_waitable_ptrs_if([this](const rclcpp::Waitable::SharedPtr & waitable) { - waitable_handles_.push_back(waitable); - return false; - }); + waitable_handles_.push_back(waitable); + return false; + }); } } return has_invalid_weak_nodes; @@ -199,8 +192,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto subscription : subscription_handles_) { if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add subscription to wait set: %s", rcl_get_error_string().str); + "rclcpp", "Couldn't add subscription to wait set: %s", rcl_get_error_string().str); return false; } } @@ -208,8 +200,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto client : client_handles_) { if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add client to wait set: %s", rcl_get_error_string().str); + "rclcpp", "Couldn't add client to wait set: %s", rcl_get_error_string().str); return false; } } @@ -217,8 +208,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto service : service_handles_) { if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add service to wait set: %s", rcl_get_error_string().str); + "rclcpp", "Couldn't add service to wait set: %s", rcl_get_error_string().str); return false; } } @@ -226,8 +216,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto timer : timer_handles_) { if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add timer to wait set: %s", rcl_get_error_string().str); + "rclcpp", "Couldn't add timer to wait set: %s", rcl_get_error_string().str); return false; } } @@ -235,9 +224,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto guard_condition : guard_conditions_) { if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add guard_condition to wait set: %s", - rcl_get_error_string().str); + "rclcpp", "Couldn't add guard_condition to wait set: %s", rcl_get_error_string().str); return false; } } @@ -245,18 +232,15 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto waitable : waitable_handles_) { if (!waitable->add_to_wait_set(wait_set)) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add waitable to wait set: %s", rcl_get_error_string().str); + "rclcpp", "Couldn't add waitable to wait set: %s", rcl_get_error_string().str); return false; } } return true; } - virtual void - get_next_subscription( - executor::AnyExecutable & any_exec, - const WeakNodeList & weak_nodes) + virtual void get_next_subscription( + executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) { auto it = subscription_handles_.begin(); while (it != subscription_handles_.end()) { @@ -297,10 +281,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - virtual void - get_next_service( - executor::AnyExecutable & any_exec, - const WeakNodeList & weak_nodes) + virtual void get_next_service(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) { auto it = service_handles_.begin(); while (it != service_handles_.end()) { @@ -332,8 +313,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - virtual void - get_next_client(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) + virtual void get_next_client(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) { auto it = client_handles_.begin(); while (it != client_handles_.end()) { @@ -365,10 +345,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - virtual void - get_next_timer( - executor::AnyExecutable & any_exec, - const WeakNodeList & weak_nodes) + virtual void get_next_timer(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) { auto it = timer_handles_.begin(); while (it != timer_handles_.end()) { @@ -400,8 +377,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - virtual void - get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) + virtual void get_next_waitable( + executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) { auto it = waitable_handles_.begin(); while (it != waitable_handles_.end()) { diff --git a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp index 0eb1d54b92..b34ae20a71 100644 --- a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp @@ -38,19 +38,15 @@ namespace message_pool_memory_strategy template< typename MessageT, size_t Size, - typename std::enable_if< - rosidl_generator_traits::has_fixed_size::value - >::type * = nullptr -> -class MessagePoolMemoryStrategy - : public message_memory_strategy::MessageMemoryStrategy + typename std::enable_if::value>::type * = + nullptr> +class MessagePoolMemoryStrategy : public message_memory_strategy::MessageMemoryStrategy { public: RCLCPP_SMART_PTR_DEFINITIONS(MessagePoolMemoryStrategy) /// Default constructor - MessagePoolMemoryStrategy() - : next_array_index_(0) + MessagePoolMemoryStrategy() : next_array_index_(0) { for (size_t i = 0; i < Size; ++i) { pool_[i].msg_ptr_ = std::make_shared(); @@ -72,7 +68,7 @@ class MessagePoolMemoryStrategy throw std::runtime_error("Tried to access message that was still in use! Abort."); } pool_[current_index].msg_ptr_->~MessageT(); - new (pool_[current_index].msg_ptr_.get())MessageT; + new (pool_[current_index].msg_ptr_.get()) MessageT; pool_[current_index].used = true; return pool_[current_index].msg_ptr_; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index e963e856ff..8c08905e6e 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -25,7 +25,6 @@ #include #include - #include "rcl/error_handling.h" #include "rcl/subscription.h" @@ -53,9 +52,7 @@ class NodeTopicsInterface; } // namespace node_interfaces /// Subscription implementation, templated on the type of message this subscription receives. -template< - typename CallbackMessageT, - typename Alloc = std::allocator> +template> class Subscription : public SubscriptionBase { friend class rclcpp::node_interfaces::NodeTopicsInterface; @@ -88,8 +85,8 @@ class Subscription : public SubscriptionBase AnySubscriptionCallback callback, const SubscriptionEventCallbacks & event_callbacks, typename message_memory_strategy::MessageMemoryStrategy::SharedPtr - memory_strategy = message_memory_strategy::MessageMemoryStrategy::create_default()) + memory_strategy = + message_memory_strategy::MessageMemoryStrategy::create_default()) : SubscriptionBase( node_handle, type_support_handle, @@ -101,13 +98,11 @@ class Subscription : public SubscriptionBase { if (event_callbacks.deadline_callback) { this->add_event_handler( - event_callbacks.deadline_callback, - RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + event_callbacks.deadline_callback, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); } if (event_callbacks.liveliness_callback) { this->add_event_handler( - event_callbacks.liveliness_callback, - RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + event_callbacks.liveliness_callback, RCL_SUBSCRIPTION_LIVELINESS_CHANGED); } } @@ -117,8 +112,8 @@ class Subscription : public SubscriptionBase * \param[in] message_memory_strategy Shared pointer to the memory strategy to set. */ void set_message_memory_strategy( - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy) + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr + message_memory_strategy) { message_memory_strategy_ = message_memory_strategy; } @@ -162,8 +157,7 @@ class Subscription : public SubscriptionBase } void handle_intra_process_message( - rcl_interfaces::msg::IntraProcessMessage & ipm, - const rmw_message_info_t & message_info) + rcl_interfaces::msg::IntraProcessMessage & ipm, const rmw_message_info_t & message_info) { if (!use_intra_process_) { // throw std::runtime_error( @@ -182,10 +176,7 @@ class Subscription : public SubscriptionBase if (any_callback_.use_take_shared_method()) { ConstMessageSharedPtr msg; take_intra_process_message( - ipm.publisher_id, - ipm.message_sequence, - intra_process_subscription_id_, - msg); + ipm.publisher_id, ipm.message_sequence, intra_process_subscription_id_, msg); if (!msg) { // This can happen when having two nodes in different process both using intraprocess // communication. It could happen too if the publisher no longer exists or the requested @@ -198,10 +189,7 @@ class Subscription : public SubscriptionBase } else { MessageUniquePtr msg; take_intra_process_message( - ipm.publisher_id, - ipm.message_sequence, - intra_process_subscription_id_, - msg); + ipm.publisher_id, ipm.message_sequence, intra_process_subscription_id_, msg); if (!msg) { // This can happen when having two nodes in different process both using intraprocess // communication. It could happen too if the publisher no longer exists or the requested @@ -215,8 +203,7 @@ class Subscription : public SubscriptionBase } /// Implemenation detail. - const std::shared_ptr - get_intra_process_subscription_handle() const + const std::shared_ptr get_intra_process_subscription_handle() const { if (!use_intra_process_) { return nullptr; @@ -225,8 +212,7 @@ class Subscription : public SubscriptionBase } private: - void - take_intra_process_message( + void take_intra_process_message( uint64_t publisher_id, uint64_t message_sequence, uint64_t subscription_id, @@ -235,14 +221,13 @@ class Subscription : public SubscriptionBase auto ipm = weak_ipm_.lock(); if (!ipm) { throw std::runtime_error( - "intra process take called after destruction of intra process manager"); + "intra process take called after destruction of intra process manager"); } ipm->template take_intra_process_message( publisher_id, message_sequence, subscription_id, message); } - void - take_intra_process_message( + void take_intra_process_message( uint64_t publisher_id, uint64_t message_sequence, uint64_t subscription_id, @@ -251,14 +236,13 @@ class Subscription : public SubscriptionBase auto ipm = weak_ipm_.lock(); if (!ipm) { throw std::runtime_error( - "intra process take called after destruction of intra process manager"); + "intra process take called after destruction of intra process manager"); } ipm->template take_intra_process_message( publisher_id, message_sequence, subscription_id, message); } - bool - matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) + bool matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) { if (!use_intra_process_) { return false; @@ -266,8 +250,8 @@ class Subscription : public SubscriptionBase auto ipm = weak_ipm_.lock(); if (!ipm) { throw std::runtime_error( - "intra process publisher check called " - "after destruction of intra process manager"); + "intra process publisher check called " + "after destruction of intra process manager"); } return ipm->matches_any_publishers(sender_gid); } diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 35255dad13..760c5cf3ac 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -46,7 +46,7 @@ namespace intra_process_manager * `intra_process_manager.hpp` and `subscription_base.hpp`. */ class IntraProcessManager; -} +} // namespace intra_process_manager /// Virtual base class for subscriptions. This pattern allows us to iterate over different template /// specializations of Subscription, among other things. @@ -77,26 +77,21 @@ class SubscriptionBase /// Get the topic that this subscription is subscribed on. RCLCPP_PUBLIC - const char * - get_topic_name() const; + const char * get_topic_name() const; RCLCPP_PUBLIC - std::shared_ptr - get_subscription_handle(); + std::shared_ptr get_subscription_handle(); RCLCPP_PUBLIC - const std::shared_ptr - get_subscription_handle() const; + const std::shared_ptr get_subscription_handle() const; RCLCPP_PUBLIC - virtual const std::shared_ptr - get_intra_process_subscription_handle() const; + virtual const std::shared_ptr get_intra_process_subscription_handle() const; /// Get all the QoS event handlers associated with this subscription. /** \return The vector of QoS event handlers. */ RCLCPP_PUBLIC - const std::vector> & - get_event_handlers() const; + const std::vector> & get_event_handlers() const; /// Get the actual QoS settings, after the defaults have been determined. /** @@ -109,53 +104,43 @@ class SubscriptionBase * \return The actual qos settings. */ RCLCPP_PUBLIC - rmw_qos_profile_t - get_actual_qos() const; + rmw_qos_profile_t get_actual_qos() const; /// Borrow a new message. /** \return Shared pointer to the fresh message. */ - virtual std::shared_ptr - create_message() = 0; + virtual std::shared_ptr create_message() = 0; /// Borrow a new serialized message /** \return Shared pointer to a rcl_message_serialized_t. */ - virtual std::shared_ptr - create_serialized_message() = 0; + virtual std::shared_ptr create_serialized_message() = 0; /// Check if we need to handle the message, and execute the callback if we do. /** * \param[in] message Shared pointer to the message to handle. * \param[in] message_info Metadata associated with this message. */ - virtual void - handle_message(std::shared_ptr & message, const rmw_message_info_t & message_info) = 0; + virtual void handle_message( + std::shared_ptr & message, const rmw_message_info_t & message_info) = 0; /// Return the message borrowed in create_message. /** \param[in] message Shared pointer to the returned message. */ - virtual void - return_message(std::shared_ptr & message) = 0; + virtual void return_message(std::shared_ptr & message) = 0; /// Return the message borrowed in create_serialized_message. /** \param[in] message Shared pointer to the returned message. */ - virtual void - return_serialized_message(std::shared_ptr & message) = 0; + virtual void return_serialized_message(std::shared_ptr & message) = 0; - virtual void - handle_intra_process_message( - rcl_interfaces::msg::IntraProcessMessage & ipm, - const rmw_message_info_t & message_info) = 0; + virtual void handle_intra_process_message( + rcl_interfaces::msg::IntraProcessMessage & ipm, const rmw_message_info_t & message_info) = 0; - const rosidl_message_type_support_t & - get_message_type_support_handle() const; + const rosidl_message_type_support_t & get_message_type_support_handle() const; - bool - is_serialized() const; + bool is_serialized() const; /// Get matching publisher count. /** \return The number of publishers on this topic. */ RCLCPP_PUBLIC - size_t - get_publisher_count() const; + size_t get_publisher_count() const; using IntraProcessManagerWeakPtr = std::weak_ptr; @@ -168,16 +153,11 @@ class SubscriptionBase protected: template - void - add_event_handler( - const EventCallbackT & callback, - const rcl_subscription_event_type_t event_type) + void add_event_handler( + const EventCallbackT & callback, const rcl_subscription_event_type_t event_type) { auto handler = std::make_shared>( - callback, - rcl_subscription_event_init, - get_subscription_handle().get(), - event_type); + callback, rcl_subscription_event_init, get_subscription_handle().get(), event_type); event_handlers_.emplace_back(handler); } diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 9b46fe33d2..1817ce9722 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -24,10 +24,10 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" -#include "rclcpp/subscription.hpp" -#include "rclcpp/subscription_traits.hpp" #include "rclcpp/intra_process_manager.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_traits.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -47,20 +47,18 @@ namespace rclcpp struct SubscriptionFactory { // Creates a Subscription object and returns it as a SubscriptionBase. - using SubscriptionFactoryFunction = std::function< - rclcpp::SubscriptionBase::SharedPtr( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const std::string & topic_name, - const rcl_subscription_options_t & subscription_options)>; + using SubscriptionFactoryFunction = std::function; SubscriptionFactoryFunction create_typed_subscription; // Function that takes a MessageT from the intra process manager - using SetupIntraProcessFunction = std::function< - void ( - rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm, - rclcpp::SubscriptionBase::SharedPtr subscription, - const rcl_subscription_options_t & subscription_options)>; + using SetupIntraProcessFunction = std::function; SetupIntraProcessFunction setup_intra_process; }; @@ -72,13 +70,11 @@ template< typename Alloc, typename CallbackMessageT, typename SubscriptionT> -SubscriptionFactory -create_subscription_factory( +SubscriptionFactory create_subscription_factory( CallbackT && callback, const SubscriptionEventCallbacks & event_callbacks, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy< - CallbackMessageT, Alloc>::SharedPtr - msg_mem_strat, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy:: + SharedPtr msg_mem_strat, std::shared_ptr allocator) { SubscriptionFactory factory; @@ -93,29 +89,28 @@ create_subscription_factory( // factory function that creates a MessageT specific SubscriptionT factory.create_typed_subscription = [allocator, msg_mem_strat, any_subscription_callback, event_callbacks, message_alloc]( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const std::string & topic_name, - const rcl_subscription_options_t & subscription_options - ) -> rclcpp::SubscriptionBase::SharedPtr - { - auto options_copy = subscription_options; - options_copy.allocator = - rclcpp::allocator::get_rcl_allocator(*message_alloc.get()); - - using rclcpp::Subscription; - using rclcpp::SubscriptionBase; - - auto sub = Subscription::make_shared( - node_base->get_shared_rcl_node_handle(), - *rosidl_typesupport_cpp::get_message_type_support_handle(), - topic_name, - options_copy, - any_subscription_callback, - event_callbacks, - msg_mem_strat); - auto sub_base_ptr = std::dynamic_pointer_cast(sub); - return sub_base_ptr; - }; + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + const rcl_subscription_options_t & subscription_options) + -> rclcpp::SubscriptionBase::SharedPtr { + auto options_copy = subscription_options; + options_copy.allocator = + rclcpp::allocator::get_rcl_allocator(*message_alloc.get()); + + using rclcpp::Subscription; + using rclcpp::SubscriptionBase; + + auto sub = Subscription::make_shared( + node_base->get_shared_rcl_node_handle(), + *rosidl_typesupport_cpp::get_message_type_support_handle(), + topic_name, + options_copy, + any_subscription_callback, + event_callbacks, + msg_mem_strat); + auto sub_base_ptr = std::dynamic_pointer_cast(sub); + return sub_base_ptr; + }; // return the factory now that it is populated return factory; diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index d76be04ee9..ee2af3439d 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -48,18 +48,20 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase /// Optional custom allocator. std::shared_ptr allocator = nullptr; - SubscriptionOptionsWithAllocator() {} + SubscriptionOptionsWithAllocator() + { + } /// Constructor using base class as input. explicit SubscriptionOptionsWithAllocator( const SubscriptionOptionsBase & subscription_options_base) : SubscriptionOptionsBase(subscription_options_base) - {} + { + } /// Convert this class, with a rclcpp::QoS, into an rcl_subscription_options_t. template - rcl_subscription_options_t - to_rcl_subscription_options(const rclcpp::QoS & qos) const + rcl_subscription_options_t to_rcl_subscription_options(const rclcpp::QoS & qos) const { rcl_subscription_options_t result; using AllocatorTraits = std::allocator_traits; diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp index ecab458bd7..34daae4c16 100644 --- a/rclcpp/include/rclcpp/subscription_traits.hpp +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -17,8 +17,8 @@ #include -#include "rclcpp/function_traits.hpp" #include "rcl/types.h" +#include "rclcpp/function_traits.hpp" namespace rclcpp { @@ -38,26 +38,31 @@ namespace subscription_traits */ template struct is_serialized_subscription_argument : std::false_type -{}; +{ +}; template<> -struct is_serialized_subscription_argument: std::true_type -{}; +struct is_serialized_subscription_argument : std::true_type +{ +}; template<> struct is_serialized_subscription_argument> - : std::true_type -{}; +: std::true_type +{ +}; template struct is_serialized_subscription : is_serialized_subscription_argument -{}; +{ +}; template struct is_serialized_callback - : is_serialized_subscription_argument< +: is_serialized_subscription_argument< typename rclcpp::function_traits::function_traits::template argument_type<0>> -{}; +{ +}; template struct extract_message_type @@ -66,30 +71,31 @@ struct extract_message_type }; template -struct extract_message_type>: extract_message_type -{}; +struct extract_message_type> : extract_message_type +{ +}; template -struct extract_message_type>: extract_message_type -{}; +struct extract_message_type> : extract_message_type +{ +}; template< typename CallbackT, // Do not attempt if CallbackT is an integer (mistaken for depth) - typename = std::enable_if_t>>::value>, + typename = std::enable_if_t< + !std::is_integral>>::value>, // Do not attempt if CallbackT is a QoS (mistaken for qos) - typename = std::enable_if_t>>::value>, + typename = std::enable_if_t< + !std::is_base_of>>::value>, // Do not attempt if CallbackT is a rmw_qos_profile_t (mistaken for qos profile) - typename = std::enable_if_t>>::value> -> -struct has_message_type : extract_message_type< + typename = std::enable_if_t< + !std::is_same>>::value>> +struct has_message_type +: extract_message_type< typename rclcpp::function_traits::function_traits::template argument_type<0>> -{}; +{ +}; } // namespace subscription_traits } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index c10b6ce23f..0a372ecf48 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -41,9 +41,7 @@ class Time Time(const Time & rhs); RCLCPP_PUBLIC - Time( - const builtin_interfaces::msg::Time & time_msg, - rcl_clock_type_t ros_time = RCL_ROS_TIME); + Time(const builtin_interfaces::msg::Time & time_msg, rcl_clock_type_t ros_time = RCL_ROS_TIME); RCLCPP_PUBLIC explicit Time(const rcl_time_point_t & time_point); @@ -55,75 +53,59 @@ class Time operator builtin_interfaces::msg::Time() const; RCLCPP_PUBLIC - Time & - operator=(const Time & rhs); + Time & operator=(const Time & rhs); RCLCPP_PUBLIC - Time & - operator=(const builtin_interfaces::msg::Time & time_msg); + Time & operator=(const builtin_interfaces::msg::Time & time_msg); RCLCPP_PUBLIC - bool - operator==(const rclcpp::Time & rhs) const; + bool operator==(const rclcpp::Time & rhs) const; RCLCPP_PUBLIC - bool - operator!=(const rclcpp::Time & rhs) const; + bool operator!=(const rclcpp::Time & rhs) const; RCLCPP_PUBLIC - bool - operator<(const rclcpp::Time & rhs) const; + bool operator<(const rclcpp::Time & rhs) const; RCLCPP_PUBLIC - bool - operator<=(const rclcpp::Time & rhs) const; + bool operator<=(const rclcpp::Time & rhs) const; RCLCPP_PUBLIC - bool - operator>=(const rclcpp::Time & rhs) const; + bool operator>=(const rclcpp::Time & rhs) const; RCLCPP_PUBLIC - bool - operator>(const rclcpp::Time & rhs) const; + bool operator>(const rclcpp::Time & rhs) const; RCLCPP_PUBLIC - Time - operator+(const rclcpp::Duration & rhs) const; + Time operator+(const rclcpp::Duration & rhs) const; RCLCPP_PUBLIC - Duration - operator-(const rclcpp::Time & rhs) const; + Duration operator-(const rclcpp::Time & rhs) const; RCLCPP_PUBLIC - Time - operator-(const rclcpp::Duration & rhs) const; + Time operator-(const rclcpp::Duration & rhs) const; RCLCPP_PUBLIC - rcl_time_point_value_t - nanoseconds() const; + rcl_time_point_value_t nanoseconds() const; RCLCPP_PUBLIC - static Time - max(); + static Time max(); /// \return the seconds since epoch as a floating point number. /// \warning Depending on sizeof(double) there could be significant precision loss. /// When an exact time is required use nanoseconds() instead. RCLCPP_PUBLIC - double - seconds() const; + double seconds() const; RCLCPP_PUBLIC - rcl_clock_type_t - get_clock_type() const; + rcl_clock_type_t get_clock_type() const; private: rcl_time_point_t rcl_time_; friend Clock; // Allow clock to manipulate internal data }; -Time -operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs); +Time operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs); } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 4dc46ce3e6..5ee0181856 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -21,12 +21,11 @@ #include "rcl/time.h" #include "builtin_interfaces/msg/time.hpp" -#include "rosgraph_msgs/msg/clock.hpp" #include "rcl_interfaces/msg/parameter_event.hpp" +#include "rosgraph_msgs/msg/clock.hpp" #include "rclcpp/node.hpp" - namespace rclcpp { class Clock; @@ -107,7 +106,7 @@ class TimeSource void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); // An enum to hold the parameter state - enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE}; + enum UseSimTimeParameterState { UNSET, SET_TRUE, SET_FALSE }; UseSimTimeParameterState parameter_state_; // An internal method to use in the clock callback that iterates and enables all clocks diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 8a6bc2291f..84c4c2cb84 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -47,16 +47,13 @@ class TimerBase RCLCPP_PUBLIC explicit TimerBase( - Clock::SharedPtr clock, - std::chrono::nanoseconds period, - rclcpp::Context::SharedPtr context); + Clock::SharedPtr clock, std::chrono::nanoseconds period, rclcpp::Context::SharedPtr context); RCLCPP_PUBLIC ~TimerBase(); RCLCPP_PUBLIC - void - cancel(); + void cancel(); /// Return the timer cancellation state. /** @@ -65,26 +62,21 @@ class TimerBase * \throws RCLErrorBase some child class exception based on ret */ RCLCPP_PUBLIC - bool - is_canceled(); + bool is_canceled(); RCLCPP_PUBLIC - void - reset(); + void reset(); RCLCPP_PUBLIC - virtual void - execute_callback() = 0; + virtual void execute_callback() = 0; RCLCPP_PUBLIC - std::shared_ptr - get_timer_handle(); + std::shared_ptr get_timer_handle(); /// Check how long the timer has until its next scheduled callback. /** \return A std::chrono::duration representing the relative time until the next callback. */ RCLCPP_PUBLIC - std::chrono::nanoseconds - time_until_trigger(); + std::chrono::nanoseconds time_until_trigger(); /// Is the clock steady (i.e. is the time between ticks constant?) /** \return True if the clock used by this timer is steady. */ @@ -104,18 +96,15 @@ class TimerBase std::shared_ptr timer_handle_; }; - -using VoidCallbackType = std::function; -using TimerCallbackType = std::function; +using VoidCallbackType = std::function; +using TimerCallbackType = std::function; /// Generic timer. Periodically executes a user-specified callback. template< typename FunctorT, typename std::enable_if< rclcpp::function_traits::same_arguments::value || - rclcpp::function_traits::same_arguments::value - >::type * = nullptr -> + rclcpp::function_traits::same_arguments::value>::type * = nullptr> class GenericTimer : public TimerBase { public: @@ -128,9 +117,10 @@ class GenericTimer : public TimerBase * \param[in] callback User-specified callback function. */ explicit GenericTimer( - Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback, - rclcpp::Context::SharedPtr context - ) + Clock::SharedPtr clock, + std::chrono::nanoseconds period, + FunctorT && callback, + rclcpp::Context::SharedPtr context) : TimerBase(clock, period, context), callback_(std::forward(callback)) { } @@ -142,8 +132,7 @@ class GenericTimer : public TimerBase cancel(); } - void - execute_callback() override + void execute_callback() override { rcl_ret_t ret = rcl_timer_call(timer_handle_.get()); if (ret == RCL_RET_TIMER_CANCELED) { @@ -159,11 +148,9 @@ class GenericTimer : public TimerBase template< typename CallbackT = FunctorT, typename std::enable_if< - rclcpp::function_traits::same_arguments::value - >::type * = nullptr - > - void - execute_callback_delegate() + rclcpp::function_traits::same_arguments::value>::type * = + nullptr> + void execute_callback_delegate() { callback_(); } @@ -171,17 +158,14 @@ class GenericTimer : public TimerBase template< typename CallbackT = FunctorT, typename std::enable_if< - rclcpp::function_traits::same_arguments::value - >::type * = nullptr - > - void - execute_callback_delegate() + rclcpp::function_traits::same_arguments::value>::type * = + nullptr> + void execute_callback_delegate() { callback_(*this); } - bool - is_steady() override + bool is_steady() override { return clock_->get_clock_type() == RCL_STEADY_TIME; } @@ -196,21 +180,18 @@ template< typename FunctorT, typename std::enable_if< rclcpp::function_traits::same_arguments::value || - rclcpp::function_traits::same_arguments::value - >::type * = nullptr -> + rclcpp::function_traits::same_arguments::value>::type * = nullptr> class WallTimer : public GenericTimer { public: RCLCPP_SMART_PTR_DEFINITIONS(WallTimer) WallTimer( - std::chrono::nanoseconds period, - FunctorT && callback, - rclcpp::Context::SharedPtr context) + std::chrono::nanoseconds period, FunctorT && callback, rclcpp::Context::SharedPtr context) : GenericTimer( std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context) - {} + { + } protected: RCLCPP_DISABLE_COPY(WallTimer) diff --git a/rclcpp/include/rclcpp/type_support_decl.hpp b/rclcpp/include/rclcpp/type_support_decl.hpp index cda0bae730..d7a3922604 100644 --- a/rclcpp/include/rclcpp/type_support_decl.hpp +++ b/rclcpp/include/rclcpp/type_support_decl.hpp @@ -29,48 +29,37 @@ namespace type_support { RCLCPP_PUBLIC -const rosidl_message_type_support_t * -get_intra_process_message_msg_type_support(); +const rosidl_message_type_support_t * get_intra_process_message_msg_type_support(); RCLCPP_PUBLIC -const rosidl_message_type_support_t * -get_parameter_event_msg_type_support(); +const rosidl_message_type_support_t * get_parameter_event_msg_type_support(); RCLCPP_PUBLIC -const rosidl_message_type_support_t * -get_set_parameters_result_msg_type_support(); +const rosidl_message_type_support_t * get_set_parameters_result_msg_type_support(); RCLCPP_PUBLIC -const rosidl_message_type_support_t * -get_parameter_descriptor_msg_type_support(); +const rosidl_message_type_support_t * get_parameter_descriptor_msg_type_support(); RCLCPP_PUBLIC -const rosidl_message_type_support_t * -get_list_parameters_result_msg_type_support(); +const rosidl_message_type_support_t * get_list_parameters_result_msg_type_support(); RCLCPP_PUBLIC -const rosidl_service_type_support_t * -get_get_parameters_srv_type_support(); +const rosidl_service_type_support_t * get_get_parameters_srv_type_support(); RCLCPP_PUBLIC -const rosidl_service_type_support_t * -get_get_parameter_types_srv_type_support(); +const rosidl_service_type_support_t * get_get_parameter_types_srv_type_support(); RCLCPP_PUBLIC -const rosidl_service_type_support_t * -get_set_parameters_srv_type_support(); +const rosidl_service_type_support_t * get_set_parameters_srv_type_support(); RCLCPP_PUBLIC -const rosidl_service_type_support_t * -get_list_parameters_srv_type_support(); +const rosidl_service_type_support_t * get_list_parameters_srv_type_support(); RCLCPP_PUBLIC -const rosidl_service_type_support_t * -get_describe_parameters_srv_type_support(); +const rosidl_service_type_support_t * get_describe_parameters_srv_type_support(); RCLCPP_PUBLIC -const rosidl_service_type_support_t * -get_set_parameters_atomically_srv_type_support(); +const rosidl_service_type_support_t * get_set_parameters_atomically_srv_type_support(); } // namespace type_support } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 606636ed0a..e4589bdb17 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -37,7 +37,7 @@ std::string to_string(T value) os << value; return os.str(); } -} +} // namespace std #endif namespace rclcpp @@ -52,8 +52,7 @@ namespace rclcpp * \sa rclcpp::Context::init() for more details on arguments and possible exceptions */ RCLCPP_PUBLIC -void -init(int argc, char const * const argv[], const InitOptions & init_options = InitOptions()); +void init(int argc, char const * const argv[], const InitOptions & init_options = InitOptions()); /// Install the global signal handler for rclcpp. /** @@ -70,13 +69,11 @@ init(int argc, char const * const argv[], const InitOptions & init_options = Ini * \return true if signal handler was installed by this function, false if already installed. */ RCLCPP_PUBLIC -bool -install_signal_handlers(); +bool install_signal_handlers(); /// Return true if the signal handlers are installed, otherwise false. RCLCPP_PUBLIC -bool -signal_handlers_installed(); +bool signal_handlers_installed(); /// Uninstall the global signal handler for rclcpp. /** @@ -92,8 +89,7 @@ signal_handlers_installed(); * \return true if signal handler was uninstalled by this function, false if was not installed. */ RCLCPP_PUBLIC -bool -uninstall_signal_handlers(); +bool uninstall_signal_handlers(); /// Initialize communications via the rmw implementation and set up a global signal handler. /** @@ -104,11 +100,8 @@ uninstall_signal_handlers(); * \throws anything remove_ros_arguments can throw */ RCLCPP_PUBLIC -std::vector -init_and_remove_ros_arguments( - int argc, - char const * const argv[], - const InitOptions & init_options = InitOptions()); +std::vector init_and_remove_ros_arguments( + int argc, char const * const argv[], const InitOptions & init_options = InitOptions()); /// Remove ROS-specific arguments from argument vector. /** @@ -124,8 +117,7 @@ init_and_remove_ros_arguments( * \throws rclcpp::exceptions::RCLErrorBase if the parsing fails */ RCLCPP_PUBLIC -std::vector -remove_ros_arguments(int argc, char const * const argv[]); +std::vector remove_ros_arguments(int argc, char const * const argv[]); /// Check rclcpp's status. /** @@ -140,8 +132,7 @@ remove_ros_arguments(int argc, char const * const argv[]); * \return true if shutdown has been called, false otherwise */ RCLCPP_PUBLIC -bool -ok(rclcpp::Context::SharedPtr context = nullptr); +bool ok(rclcpp::Context::SharedPtr context = nullptr); /// Return true if init() has already been called for the given context. /** @@ -154,8 +145,7 @@ ok(rclcpp::Context::SharedPtr context = nullptr); * \return true if the context is initialized, and false otherwise */ RCLCPP_PUBLIC -bool -is_initialized(rclcpp::Context::SharedPtr context = nullptr); +bool is_initialized(rclcpp::Context::SharedPtr context = nullptr); /// Shutdown rclcpp context, invalidating it for derived entities. /** @@ -171,8 +161,7 @@ is_initialized(rclcpp::Context::SharedPtr context = nullptr); * \return true if shutdown was successful, false if context was already shutdown */ RCLCPP_PUBLIC -bool -shutdown( +bool shutdown( rclcpp::Context::SharedPtr context = nullptr, const std::string & reason = "user called rclcpp::shutdown()"); @@ -192,8 +181,7 @@ shutdown( * \param[in] context with which to associate the context */ RCLCPP_PUBLIC -void -on_shutdown(std::function callback, rclcpp::Context::SharedPtr context = nullptr); +void on_shutdown(std::function callback, rclcpp::Context::SharedPtr context = nullptr); /// Use the global condition variable to block for the specified amount of time. /** @@ -209,10 +197,8 @@ on_shutdown(std::function callback, rclcpp::Context::SharedPtr context = * \return true if the condition variable did not timeout. */ RCLCPP_PUBLIC -bool -sleep_for( - const std::chrono::nanoseconds & nanoseconds, - rclcpp::Context::SharedPtr context = nullptr); +bool sleep_for( + const std::chrono::nanoseconds & nanoseconds, rclcpp::Context::SharedPtr context = nullptr); /// Safely check if addition will overflow. /** @@ -225,8 +211,7 @@ sleep_for( * \return True if the x + y sum is greater than T::max value. */ template -bool -add_will_overflow(const T x, const T y) +bool add_will_overflow(const T x, const T y) { return (y > 0) && (x > (std::numeric_limits::max() - y)); } @@ -242,8 +227,7 @@ add_will_overflow(const T x, const T y) * \return True if the x + y sum is less than T::min value. */ template -bool -add_will_underflow(const T x, const T y) +bool add_will_underflow(const T x, const T y) { return (y < 0) && (x < (std::numeric_limits::min() - y)); } @@ -259,8 +243,7 @@ add_will_underflow(const T x, const T y) * \return True if the difference `x - y` sum is grater than T::max value. */ template -bool -sub_will_overflow(const T x, const T y) +bool sub_will_overflow(const T x, const T y) { return (y < 0) && (x > (std::numeric_limits::max() + y)); } @@ -276,8 +259,7 @@ sub_will_overflow(const T x, const T y) * \return True if the difference `x - y` sum is less than T::min value. */ template -bool -sub_will_underflow(const T x, const T y) +bool sub_will_underflow(const T x, const T y) { return (y > 0) && (x < (std::numeric_limits::min() + y)); } @@ -290,8 +272,7 @@ sub_will_underflow(const T x, const T y) * \return the given string */ RCLCPP_PUBLIC -const char * -get_c_string(const char * string_in); +const char * get_c_string(const char * string_in); /// Return the C string from the given std::string. /** @@ -299,8 +280,7 @@ get_c_string(const char * string_in); * \return the C string from the std::string */ RCLCPP_PUBLIC -const char * -get_c_string(const std::string & string_in); +const char * get_c_string(const std::string & string_in); } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/visibility_control.hpp b/rclcpp/include/rclcpp/visibility_control.hpp index 044fedbb8b..39c6d23334 100644 --- a/rclcpp/include/rclcpp/visibility_control.hpp +++ b/rclcpp/include/rclcpp/visibility_control.hpp @@ -26,31 +26,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define RCLCPP_EXPORT __attribute__ ((dllexport)) - #define RCLCPP_IMPORT __attribute__ ((dllimport)) - #else - #define RCLCPP_EXPORT __declspec(dllexport) - #define RCLCPP_IMPORT __declspec(dllimport) - #endif - #ifdef RCLCPP_BUILDING_LIBRARY - #define RCLCPP_PUBLIC RCLCPP_EXPORT - #else - #define RCLCPP_PUBLIC RCLCPP_IMPORT - #endif - #define RCLCPP_PUBLIC_TYPE RCLCPP_PUBLIC - #define RCLCPP_LOCAL +#ifdef __GNUC__ +#define RCLCPP_EXPORT __attribute__((dllexport)) +#define RCLCPP_IMPORT __attribute__((dllimport)) #else - #define RCLCPP_EXPORT __attribute__ ((visibility("default"))) - #define RCLCPP_IMPORT - #if __GNUC__ >= 4 - #define RCLCPP_PUBLIC __attribute__ ((visibility("default"))) - #define RCLCPP_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define RCLCPP_PUBLIC - #define RCLCPP_LOCAL - #endif - #define RCLCPP_PUBLIC_TYPE +#define RCLCPP_EXPORT __declspec(dllexport) +#define RCLCPP_IMPORT __declspec(dllimport) +#endif +#ifdef RCLCPP_BUILDING_LIBRARY +#define RCLCPP_PUBLIC RCLCPP_EXPORT +#else +#define RCLCPP_PUBLIC RCLCPP_IMPORT +#endif +#define RCLCPP_PUBLIC_TYPE RCLCPP_PUBLIC +#define RCLCPP_LOCAL +#else +#define RCLCPP_EXPORT __attribute__((visibility("default"))) +#define RCLCPP_IMPORT +#if __GNUC__ >= 4 +#define RCLCPP_PUBLIC __attribute__((visibility("default"))) +#define RCLCPP_LOCAL __attribute__((visibility("hidden"))) +#else +#define RCLCPP_PUBLIC +#define RCLCPP_LOCAL +#endif +#define RCLCPP_PUBLIC_TYPE #endif #endif // RCLCPP__VISIBILITY_CONTROL_HPP_ diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 2837d5a16d..aab45e72ca 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -35,9 +35,7 @@ class Waitable * \return The number of subscriptions associated with the Waitable. */ RCLCPP_PUBLIC - virtual - size_t - get_number_of_ready_subscriptions(); + virtual size_t get_number_of_ready_subscriptions(); /// Get the number of ready timers /** @@ -46,9 +44,7 @@ class Waitable * \return The number of timers associated with the Waitable. */ RCLCPP_PUBLIC - virtual - size_t - get_number_of_ready_timers(); + virtual size_t get_number_of_ready_timers(); /// Get the number of ready clients /** @@ -57,9 +53,7 @@ class Waitable * \return The number of clients associated with the Waitable. */ RCLCPP_PUBLIC - virtual - size_t - get_number_of_ready_clients(); + virtual size_t get_number_of_ready_clients(); /// Get the number of ready events /** @@ -68,9 +62,7 @@ class Waitable * \return The number of events associated with the Waitable. */ RCLCPP_PUBLIC - virtual - size_t - get_number_of_ready_events(); + virtual size_t get_number_of_ready_events(); /// Get the number of ready services /** @@ -79,9 +71,7 @@ class Waitable * \return The number of services associated with the Waitable. */ RCLCPP_PUBLIC - virtual - size_t - get_number_of_ready_services(); + virtual size_t get_number_of_ready_services(); /// Get the number of ready guard_conditions /** @@ -90,9 +80,7 @@ class Waitable * \return The number of guard_conditions associated with the Waitable. */ RCLCPP_PUBLIC - virtual - size_t - get_number_of_ready_guard_conditions(); + virtual size_t get_number_of_ready_guard_conditions(); // TODO(jacobperron): smart pointer? /// Add the Waitable to a wait set. @@ -102,9 +90,7 @@ class Waitable * \throws rclcpp::execptions::RCLError from rcl_wait_set_add_*() */ RCLCPP_PUBLIC - virtual - bool - add_to_wait_set(rcl_wait_set_t * wait_set) = 0; + virtual bool add_to_wait_set(rcl_wait_set_t * wait_set) = 0; /// Check if the Waitable is ready. /** @@ -117,9 +103,7 @@ class Waitable * \return `true` if the Waitable is ready, `false` otherwise. */ RCLCPP_PUBLIC - virtual - bool - is_ready(rcl_wait_set_t *) = 0; + virtual bool is_ready(rcl_wait_set_t *) = 0; /// Execute any entities of the Waitable that are ready. /** @@ -143,9 +127,7 @@ class Waitable * ``` */ RCLCPP_PUBLIC - virtual - void - execute() = 0; + virtual void execute() = 0; }; // class Waitable } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 256033bbad..4059fce1a3 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -24,7 +24,8 @@ AnyExecutable::AnyExecutable() client(nullptr), callback_group(nullptr), node_base(nullptr) -{} +{ +} AnyExecutable::~AnyExecutable() { diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index e9d0e80c48..240a056857 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -21,24 +21,20 @@ using rclcpp::callback_group::CallbackGroupType; CallbackGroup::CallbackGroup(CallbackGroupType group_type) : type_(group_type), can_be_taken_from_(true) -{} - +{ +} -std::atomic_bool & -CallbackGroup::can_be_taken_from() +std::atomic_bool & CallbackGroup::can_be_taken_from() { return can_be_taken_from_; } -const CallbackGroupType & -CallbackGroup::type() const +const CallbackGroupType & CallbackGroup::type() const { return type_; } -void -CallbackGroup::add_subscription( - const rclcpp::SubscriptionBase::SharedPtr subscription_ptr) +void CallbackGroup::add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr) { std::lock_guard lock(mutex_); subscription_ptrs_.push_back(subscription_ptr); @@ -46,12 +42,11 @@ CallbackGroup::add_subscription( std::remove_if( subscription_ptrs_.begin(), subscription_ptrs_.end(), - [](rclcpp::SubscriptionBase::WeakPtr x) {return x.expired();}), + [](rclcpp::SubscriptionBase::WeakPtr x) { return x.expired(); }), subscription_ptrs_.end()); } -void -CallbackGroup::add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr) +void CallbackGroup::add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr) { std::lock_guard lock(mutex_); timer_ptrs_.push_back(timer_ptr); @@ -59,12 +54,11 @@ CallbackGroup::add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr) std::remove_if( timer_ptrs_.begin(), timer_ptrs_.end(), - [](rclcpp::TimerBase::WeakPtr x) {return x.expired();}), + [](rclcpp::TimerBase::WeakPtr x) { return x.expired(); }), timer_ptrs_.end()); } -void -CallbackGroup::add_service(const rclcpp::ServiceBase::SharedPtr service_ptr) +void CallbackGroup::add_service(const rclcpp::ServiceBase::SharedPtr service_ptr) { std::lock_guard lock(mutex_); service_ptrs_.push_back(service_ptr); @@ -72,12 +66,11 @@ CallbackGroup::add_service(const rclcpp::ServiceBase::SharedPtr service_ptr) std::remove_if( service_ptrs_.begin(), service_ptrs_.end(), - [](rclcpp::ServiceBase::WeakPtr x) {return x.expired();}), + [](rclcpp::ServiceBase::WeakPtr x) { return x.expired(); }), service_ptrs_.end()); } -void -CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr) +void CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr) { std::lock_guard lock(mutex_); client_ptrs_.push_back(client_ptr); @@ -85,12 +78,11 @@ CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr) std::remove_if( client_ptrs_.begin(), client_ptrs_.end(), - [](rclcpp::ClientBase::WeakPtr x) {return x.expired();}), + [](rclcpp::ClientBase::WeakPtr x) { return x.expired(); }), client_ptrs_.end()); } -void -CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) +void CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) { std::lock_guard lock(mutex_); waitable_ptrs_.push_back(waitable_ptr); @@ -98,12 +90,11 @@ CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) std::remove_if( waitable_ptrs_.begin(), waitable_ptrs_.end(), - [](rclcpp::Waitable::WeakPtr x) {return x.expired();}), + [](rclcpp::Waitable::WeakPtr x) { return x.expired(); }), waitable_ptrs_.end()); } -void -CallbackGroup::remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept +void CallbackGroup::remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept { std::lock_guard lock(mutex_); for (auto iter = waitable_ptrs_.begin(); iter != waitable_ptrs_.end(); ++iter) { diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index c2f64e06f7..b16e7bdbb0 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -24,10 +24,10 @@ #include "rcl/node.h" #include "rcl/wait.h" #include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/logging.hpp" using rclcpp::ClientBase; using rclcpp::exceptions::InvalidNodeError; @@ -43,25 +43,24 @@ ClientBase::ClientBase( std::weak_ptr weak_node_handle(node_handle_); rcl_client_t * new_rcl_client = new rcl_client_t; *new_rcl_client = rcl_get_zero_initialized_client(); - client_handle_.reset( - new_rcl_client, [weak_node_handle](rcl_client_t * client) - { - auto handle = weak_node_handle.lock(); - if (handle) { - if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) { - RCLCPP_ERROR( - rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), - "Error in destruction of rcl client handle: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - } else { + client_handle_.reset(new_rcl_client, [weak_node_handle](rcl_client_t * client) { + auto handle = weak_node_handle.lock(); + if (handle) { + if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) { RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "Error in destruction of rcl client handle: " - "the Node Handle was destructed too early. You will leak memory"); + rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), + "Error in destruction of rcl client handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); } - delete client; - }); + } else { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Error in destruction of rcl client handle: " + "the Node Handle was destructed too early. You will leak memory"); + } + delete client; + }); } ClientBase::~ClientBase() @@ -70,32 +69,26 @@ ClientBase::~ClientBase() client_handle_.reset(); } -const char * -ClientBase::get_service_name() const +const char * ClientBase::get_service_name() const { return rcl_client_get_service_name(this->get_client_handle().get()); } -std::shared_ptr -ClientBase::get_client_handle() +std::shared_ptr ClientBase::get_client_handle() { return client_handle_; } -std::shared_ptr -ClientBase::get_client_handle() const +std::shared_ptr ClientBase::get_client_handle() const { return client_handle_; } -bool -ClientBase::service_is_ready() const +bool ClientBase::service_is_ready() const { bool is_ready; rcl_ret_t ret = rcl_service_server_is_available( - this->get_rcl_node_handle(), - this->get_client_handle().get(), - &is_ready); + this->get_rcl_node_handle(), this->get_client_handle().get(), &is_ready); if (RCL_RET_NODE_INVALID == ret) { const rcl_node_t * node_handle = this->get_rcl_node_handle(); if (node_handle && !rcl_context_is_valid(node_handle->context)) { @@ -109,8 +102,7 @@ ClientBase::service_is_ready() const return is_ready; } -bool -ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) +bool ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) { auto start = std::chrono::steady_clock::now(); // make an event to reuse, rather than create a new one each time @@ -129,10 +121,9 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) auto event = node_ptr->get_graph_event(); // update the time even on the first loop to account for time spent in the first call // to this->server_is_ready() - std::chrono::nanoseconds time_to_wait = - timeout > std::chrono::nanoseconds(0) ? - timeout - (std::chrono::steady_clock::now() - start) : - std::chrono::nanoseconds::max(); + std::chrono::nanoseconds time_to_wait = timeout > std::chrono::nanoseconds(0) ? + timeout - (std::chrono::steady_clock::now() - start) : + std::chrono::nanoseconds::max(); if (time_to_wait < std::chrono::nanoseconds(0)) { // Do not allow the time_to_wait to become negative when timeout was originally positive. // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while. @@ -166,14 +157,12 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) return false; // timeout exceeded while waiting for the server to be ready } -rcl_node_t * -ClientBase::get_rcl_node_handle() +rcl_node_t * ClientBase::get_rcl_node_handle() { return node_handle_.get(); } -const rcl_node_t * -ClientBase::get_rcl_node_handle() const +const rcl_node_t * ClientBase::get_rcl_node_handle() const { return node_handle_.get(); } diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index a0e02cc362..2b7f45e44e 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -25,10 +25,9 @@ JumpHandler::JumpHandler( pre_callback_t pre_callback, post_callback_t post_callback, const rcl_jump_threshold_t & threshold) -: pre_callback(pre_callback), - post_callback(post_callback), - notice_threshold(threshold) -{} +: pre_callback(pre_callback), post_callback(post_callback), notice_threshold(threshold) +{ +} Clock::Clock(rcl_clock_type_t clock_type) { @@ -47,8 +46,7 @@ Clock::~Clock() } } -Time -Clock::now() +Time Clock::now() { Time now(0, 0, rcl_clock_.type); @@ -60,8 +58,7 @@ Clock::now() return now; } -bool -Clock::ros_time_is_active() +bool Clock::ros_time_is_active() { if (!rcl_clock_valid(&rcl_clock_)) { RCUTILS_LOG_ERROR("ROS time not valid!"); @@ -71,29 +68,23 @@ Clock::ros_time_is_active() bool is_enabled = false; auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled); if (ret != RCL_RET_OK) { - exceptions::throw_from_rcl_error( - ret, "Failed to check ros_time_override_status"); + exceptions::throw_from_rcl_error(ret, "Failed to check ros_time_override_status"); } return is_enabled; } -rcl_clock_t * -Clock::get_clock_handle() noexcept +rcl_clock_t * Clock::get_clock_handle() noexcept { return &rcl_clock_; } -rcl_clock_type_t -Clock::get_clock_type() const noexcept +rcl_clock_type_t Clock::get_clock_type() const noexcept { return rcl_clock_.type; } -void -Clock::on_time_jump( - const struct rcl_time_jump_t * time_jump, - bool before_jump, - void * user_data) +void Clock::on_time_jump( + const struct rcl_time_jump_t * time_jump, bool before_jump, void * user_data) { const auto * handler = static_cast(user_data); if (nullptr == handler) { @@ -106,8 +97,7 @@ Clock::on_time_jump( } } -JumpHandler::SharedPtr -Clock::create_jump_callback( +JumpHandler::SharedPtr Clock::create_jump_callback( JumpHandler::pre_callback_t pre_callback, JumpHandler::post_callback_t post_callback, const rcl_jump_threshold_t & threshold) @@ -119,9 +109,8 @@ Clock::create_jump_callback( } // Try to add the jump callback to the clock - rcl_ret_t ret = rcl_clock_add_jump_callback( - &rcl_clock_, threshold, Clock::on_time_jump, - handler.get()); + rcl_ret_t ret = + rcl_clock_add_jump_callback(&rcl_clock_, threshold, Clock::on_time_jump, handler.get()); if (RCL_RET_OK != ret) { exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback"); } @@ -130,8 +119,7 @@ Clock::create_jump_callback( // create shared_ptr that removes the callback automatically when all copies are destructed // TODO(dorezyuk) UB, if the clock leaves scope before the JumpHandler return JumpHandler::SharedPtr(handler.release(), [this](JumpHandler * handler) noexcept { - rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, Clock::on_time_jump, - handler); + rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, Clock::on_time_jump, handler); delete handler; handler = NULL; if (RCL_RET_OK != ret) { diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index a93086aa5f..295bce3760 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -31,8 +31,9 @@ static std::vector> g_contexts; using rclcpp::Context; -Context::Context() -: rcl_context_(nullptr), shutdown_reason_("") {} +Context::Context() : rcl_context_(nullptr), shutdown_reason_("") +{ +} Context::~Context() { @@ -52,8 +53,7 @@ Context::~Context() } RCLCPP_LOCAL -void -__delete_context(rcl_context_t * context) +void __delete_context(rcl_context_t * context) { if (context) { if (rcl_context_is_valid(context)) { @@ -65,7 +65,8 @@ __delete_context(rcl_context_t * context) if (RCL_RET_OK != ret) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), - "failed to finalize context: %s", rcl_get_error_string().str); + "failed to finalize context: %s", + rcl_get_error_string().str); rcl_reset_error(); } } @@ -73,11 +74,7 @@ __delete_context(rcl_context_t * context) } } -void -Context::init( - int argc, - char const * const argv[], - const rclcpp::InitOptions & init_options) +void Context::init(int argc, char const * const argv[], const rclcpp::InitOptions & init_options) { std::lock_guard init_lock(init_mutex_); if (this->is_valid()) { @@ -97,8 +94,7 @@ Context::init( g_contexts.push_back(this->shared_from_this()); } -bool -Context::is_valid() const +bool Context::is_valid() const { // Take a local copy of the shared pointer to avoid it getting nulled under our feet. auto local_rcl_context = rcl_context_; @@ -108,27 +104,23 @@ Context::is_valid() const return rcl_context_is_valid(local_rcl_context.get()); } -const rclcpp::InitOptions & -Context::get_init_options() const +const rclcpp::InitOptions & Context::get_init_options() const { return init_options_; } -rclcpp::InitOptions -Context::get_init_options() +rclcpp::InitOptions Context::get_init_options() { return init_options_; } -std::string -Context::shutdown_reason() +std::string Context::shutdown_reason() { std::lock_guard lock(init_mutex_); return shutdown_reason_; } -bool -Context::shutdown(const std::string & reason) +bool Context::shutdown(const std::string & reason) { // prevent races std::lock_guard init_lock(init_mutex_); @@ -153,7 +145,7 @@ Context::shutdown(const std::string & reason) this->interrupt_all_wait_sets(); // remove self from the global contexts std::lock_guard context_lock(g_contexts_mutex); - for (auto it = g_contexts.begin(); it != g_contexts.end(); ) { + for (auto it = g_contexts.begin(); it != g_contexts.end();) { auto shared_context = it->lock(); if (shared_context.get() == this) { it = g_contexts.erase(it); @@ -165,33 +157,28 @@ Context::shutdown(const std::string & reason) return true; } -rclcpp::Context::OnShutdownCallback -Context::on_shutdown(OnShutdownCallback callback) +rclcpp::Context::OnShutdownCallback Context::on_shutdown(OnShutdownCallback callback) { on_shutdown_callbacks_.push_back(callback); return callback; } -const std::vector & -Context::get_on_shutdown_callbacks() const +const std::vector & Context::get_on_shutdown_callbacks() const { return on_shutdown_callbacks_; } -std::vector & -Context::get_on_shutdown_callbacks() +std::vector & Context::get_on_shutdown_callbacks() { return on_shutdown_callbacks_; } -std::shared_ptr -Context::get_rcl_context() +std::shared_ptr Context::get_rcl_context() { return rcl_context_; } -bool -Context::sleep_for(const std::chrono::nanoseconds & nanoseconds) +bool Context::sleep_for(const std::chrono::nanoseconds & nanoseconds) { std::chrono::nanoseconds time_left = nanoseconds; { @@ -208,14 +195,12 @@ Context::sleep_for(const std::chrono::nanoseconds & nanoseconds) return this->is_valid(); } -void -Context::interrupt_all_sleep_for() +void Context::interrupt_all_sleep_for() { interrupt_condition_variable_.notify_all(); } -rcl_guard_condition_t * -Context::get_interrupt_guard_condition(rcl_wait_set_t * wait_set) +rcl_guard_condition_t * Context::get_interrupt_guard_condition(rcl_wait_set_t * wait_set) { std::lock_guard lock(interrupt_guard_cond_handles_mutex_); auto kv = interrupt_guard_cond_handles_.find(wait_set); @@ -233,8 +218,7 @@ Context::get_interrupt_guard_condition(rcl_wait_set_t * wait_set) } } -void -Context::release_interrupt_guard_condition(rcl_wait_set_t * wait_set) +void Context::release_interrupt_guard_condition(rcl_wait_set_t * wait_set) { std::lock_guard lock(interrupt_guard_cond_handles_mutex_); auto kv = interrupt_guard_cond_handles_.find(wait_set); @@ -249,10 +233,8 @@ Context::release_interrupt_guard_condition(rcl_wait_set_t * wait_set) } } -void -Context::release_interrupt_guard_condition( - rcl_wait_set_t * wait_set, - const std::nothrow_t &) noexcept +void Context::release_interrupt_guard_condition( + rcl_wait_set_t * wait_set, const std::nothrow_t &) noexcept { try { this->release_interrupt_guard_condition(wait_set); @@ -260,7 +242,8 @@ Context::release_interrupt_guard_condition( RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), "caught %s exception when releasing interrupt guard condition: %s", - rmw::impl::cpp::demangle(exc).c_str(), exc.what()); + rmw::impl::cpp::demangle(exc).c_str(), + exc.what()); } catch (...) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), @@ -268,8 +251,7 @@ Context::release_interrupt_guard_condition( } } -void -Context::interrupt_all_wait_sets() +void Context::interrupt_all_wait_sets() { std::lock_guard lock(interrupt_guard_cond_handles_mutex_); for (auto & kv : interrupt_guard_cond_handles_) { @@ -283,15 +265,13 @@ Context::interrupt_all_wait_sets() } } -void -Context::clean_up() +void Context::clean_up() { shutdown_reason_ = ""; rcl_context_.reset(); } -std::vector -rclcpp::get_contexts() +std::vector rclcpp::get_contexts() { std::lock_guard lock(g_contexts_mutex); std::vector shared_contexts; diff --git a/rclcpp/src/rclcpp/contexts/default_context.cpp b/rclcpp/src/rclcpp/contexts/default_context.cpp index 53c6c9f3f2..78af498a49 100644 --- a/rclcpp/src/rclcpp/contexts/default_context.cpp +++ b/rclcpp/src/rclcpp/contexts/default_context.cpp @@ -17,10 +17,10 @@ using rclcpp::contexts::default_context::DefaultContext; DefaultContext::DefaultContext() -{} +{ +} -DefaultContext::SharedPtr -rclcpp::contexts::default_context::get_global_default_context() +DefaultContext::SharedPtr rclcpp::contexts::default_context::get_global_default_context() { static DefaultContext::SharedPtr default_context = DefaultContext::make_shared(); return default_context; diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 35db0165f5..18c153d69f 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -52,15 +52,13 @@ Duration::Duration(const Duration & rhs) rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds; } -Duration::Duration( - const builtin_interfaces::msg::Duration & duration_msg) +Duration::Duration(const builtin_interfaces::msg::Duration & duration_msg) { rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(duration_msg.sec)); rcl_duration_.nanoseconds += duration_msg.nanosec; } -Duration::Duration(const rcl_duration_t & duration) -: rcl_duration_(duration) +Duration::Duration(const rcl_duration_t & duration) : rcl_duration_(duration) { // noop } @@ -74,110 +72,96 @@ Duration::operator builtin_interfaces::msg::Duration() const return msg_duration; } -Duration & -Duration::operator=(const Duration & rhs) +Duration & Duration::operator=(const Duration & rhs) { rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds; return *this; } -Duration & -Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg) +Duration & Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg) { rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(duration_msg.sec)); rcl_duration_.nanoseconds += duration_msg.nanosec; return *this; } -bool -Duration::operator==(const rclcpp::Duration & rhs) const +bool Duration::operator==(const rclcpp::Duration & rhs) const { return rcl_duration_.nanoseconds == rhs.rcl_duration_.nanoseconds; } -bool -Duration::operator<(const rclcpp::Duration & rhs) const +bool Duration::operator<(const rclcpp::Duration & rhs) const { return rcl_duration_.nanoseconds < rhs.rcl_duration_.nanoseconds; } -bool -Duration::operator<=(const rclcpp::Duration & rhs) const +bool Duration::operator<=(const rclcpp::Duration & rhs) const { return rcl_duration_.nanoseconds <= rhs.rcl_duration_.nanoseconds; } -bool -Duration::operator>=(const rclcpp::Duration & rhs) const +bool Duration::operator>=(const rclcpp::Duration & rhs) const { return rcl_duration_.nanoseconds >= rhs.rcl_duration_.nanoseconds; } -bool -Duration::operator>(const rclcpp::Duration & rhs) const +bool Duration::operator>(const rclcpp::Duration & rhs) const { return rcl_duration_.nanoseconds > rhs.rcl_duration_.nanoseconds; } -void -bounds_check_duration_sum(int64_t lhsns, int64_t rhsns, uint64_t max) +void bounds_check_duration_sum(int64_t lhsns, int64_t rhsns, uint64_t max) { auto abs_lhs = (uint64_t)std::abs(lhsns); auto abs_rhs = (uint64_t)std::abs(rhsns); if (lhsns > 0 && rhsns > 0) { - if (abs_lhs + abs_rhs > (uint64_t) max) { + if (abs_lhs + abs_rhs > (uint64_t)max) { throw std::overflow_error("addition leads to int64_t overflow"); } } else if (lhsns < 0 && rhsns < 0) { - if (abs_lhs + abs_rhs > (uint64_t) max) { + if (abs_lhs + abs_rhs > (uint64_t)max) { throw std::underflow_error("addition leads to int64_t underflow"); } } } -Duration -Duration::operator+(const rclcpp::Duration & rhs) const +Duration Duration::operator+(const rclcpp::Duration & rhs) const { bounds_check_duration_sum( this->rcl_duration_.nanoseconds, rhs.rcl_duration_.nanoseconds, std::numeric_limits::max()); - return Duration( - rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds); + return Duration(rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds); } -void -bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max) +void bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max) { auto abs_lhs = (uint64_t)std::abs(lhsns); auto abs_rhs = (uint64_t)std::abs(rhsns); if (lhsns > 0 && rhsns < 0) { - if (abs_lhs + abs_rhs > (uint64_t) max) { + if (abs_lhs + abs_rhs > (uint64_t)max) { throw std::overflow_error("duration subtraction leads to int64_t overflow"); } } else if (lhsns < 0 && rhsns > 0) { - if (abs_lhs + abs_rhs > (uint64_t) max) { + if (abs_lhs + abs_rhs > (uint64_t)max) { throw std::underflow_error("duration subtraction leads to int64_t underflow"); } } } -Duration -Duration::operator-(const rclcpp::Duration & rhs) const +Duration Duration::operator-(const rclcpp::Duration & rhs) const { bounds_check_duration_difference( this->rcl_duration_.nanoseconds, rhs.rcl_duration_.nanoseconds, std::numeric_limits::max()); - return Duration( - rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds); + return Duration(rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds); } -void -bounds_check_duration_scale(int64_t dns, double scale, uint64_t max) +void bounds_check_duration_scale(int64_t dns, double scale, uint64_t max) { auto abs_dns = static_cast(std::abs(dns)); auto abs_scale = std::abs(scale); @@ -191,39 +175,32 @@ bounds_check_duration_scale(int64_t dns, double scale, uint64_t max) } } -Duration -Duration::operator*(double scale) const +Duration Duration::operator*(double scale) const { if (!std::isfinite(scale)) { throw std::runtime_error("abnormal scale in rclcpp::Duration"); } bounds_check_duration_scale( - this->rcl_duration_.nanoseconds, - scale, - std::numeric_limits::max()); + this->rcl_duration_.nanoseconds, scale, std::numeric_limits::max()); return Duration(static_cast(rcl_duration_.nanoseconds * scale)); } -rcl_duration_value_t -Duration::nanoseconds() const +rcl_duration_value_t Duration::nanoseconds() const { return rcl_duration_.nanoseconds; } -Duration -Duration::max() +Duration Duration::max() { return Duration(std::numeric_limits::max(), 999999999); } -double -Duration::seconds() const +double Duration::seconds() const { return std::chrono::duration(std::chrono::nanoseconds(rcl_duration_.nanoseconds)).count(); } -rmw_time_t -Duration::to_rmw_time() const +rmw_time_t Duration::to_rmw_time() const { // reuse conversion logic from msg creation builtin_interfaces::msg::Duration msg = *this; @@ -233,8 +210,7 @@ Duration::to_rmw_time() const return result; } -Duration -Duration::from_seconds(double seconds) +Duration Duration::from_seconds(double seconds) { return Duration(static_cast(RCL_S_TO_NS(seconds))); } diff --git a/rclcpp/src/rclcpp/event.cpp b/rclcpp/src/rclcpp/event.cpp index 6be20a6335..fe80c9e0af 100644 --- a/rclcpp/src/rclcpp/event.cpp +++ b/rclcpp/src/rclcpp/event.cpp @@ -17,23 +17,21 @@ namespace rclcpp { -Event::Event() -: state_(false) {} +Event::Event() : state_(false) +{ +} -bool -Event::set() +bool Event::set() { return state_.exchange(true); } -bool -Event::check() +bool Event::check() { return state_.load(); } -bool -Event::check_and_clear() +bool Event::check_and_clear() { return state_.exchange(false); } diff --git a/rclcpp/src/rclcpp/exceptions.cpp b/rclcpp/src/rclcpp/exceptions.cpp index 4371066c84..61b8964c99 100644 --- a/rclcpp/src/rclcpp/exceptions.cpp +++ b/rclcpp/src/rclcpp/exceptions.cpp @@ -26,12 +26,8 @@ namespace rclcpp namespace exceptions { -std::string -NameValidationError::format_error( - const char * name_type, - const char * name, - const char * error_msg, - size_t invalid_index) +std::string NameValidationError::format_error( + const char * name_type, const char * name, const char * error_msg, size_t invalid_index) { std::string msg = ""; msg += "Invalid "s + name_type + ": " + error_msg + ":\n"; @@ -40,12 +36,11 @@ NameValidationError::format_error( return msg; } -std::exception_ptr -from_rcl_error( +std::exception_ptr from_rcl_error( rcl_ret_t ret, const std::string & prefix, const rcl_error_state_t * error_state, - void (* reset_error)()) + void (*reset_error)()) { if (RCL_RET_OK == ret) { throw std::invalid_argument("ret is RCL_RET_OK"); @@ -76,12 +71,11 @@ from_rcl_error( } } -void -throw_from_rcl_error( +void throw_from_rcl_error( rcl_ret_t ret, const std::string & prefix, const rcl_error_state_t * error_state, - void (* reset_error)()) + void (*reset_error)()) { // We expect this to either throw a standard error, // or to generate an error pointer (which is caught @@ -91,56 +85,55 @@ throw_from_rcl_error( } RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state) -: ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number), +: ret(ret), + message(error_state->message), + file(error_state->file), + line(error_state->line_number), formatted_message(rcl_get_error_string().str) -{} +{ +} -RCLError::RCLError( - rcl_ret_t ret, - const rcl_error_state_t * error_state, - const std::string & prefix) +RCLError::RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix) : RCLError(RCLErrorBase(ret, error_state), prefix) -{} +{ +} -RCLError::RCLError( - const RCLErrorBase & base_exc, - const std::string & prefix) +RCLError::RCLError(const RCLErrorBase & base_exc, const std::string & prefix) : RCLErrorBase(base_exc), std::runtime_error(prefix + base_exc.formatted_message) -{} +{ +} RCLBadAlloc::RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state) : RCLBadAlloc(RCLErrorBase(ret, error_state)) -{} +{ +} -RCLBadAlloc::RCLBadAlloc(const RCLErrorBase & base_exc) -: RCLErrorBase(base_exc), std::bad_alloc() -{} +RCLBadAlloc::RCLBadAlloc(const RCLErrorBase & base_exc) : RCLErrorBase(base_exc), std::bad_alloc() +{ +} RCLInvalidArgument::RCLInvalidArgument( - rcl_ret_t ret, - const rcl_error_state_t * error_state, - const std::string & prefix) + rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix) : RCLInvalidArgument(RCLErrorBase(ret, error_state), prefix) -{} +{ +} -RCLInvalidArgument::RCLInvalidArgument( - const RCLErrorBase & base_exc, - const std::string & prefix) +RCLInvalidArgument::RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix) : RCLErrorBase(base_exc), std::invalid_argument(prefix + base_exc.formatted_message) -{} +{ +} RCLInvalidROSArgsError::RCLInvalidROSArgsError( - rcl_ret_t ret, - const rcl_error_state_t * error_state, - const std::string & prefix) + rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix) : RCLInvalidROSArgsError(RCLErrorBase(ret, error_state), prefix) -{} +{ +} RCLInvalidROSArgsError::RCLInvalidROSArgsError( - const RCLErrorBase & base_exc, - const std::string & prefix) + const RCLErrorBase & base_exc, const std::string & prefix) : RCLErrorBase(base_exc), std::runtime_error(prefix + base_exc.formatted_message) -{} +{ +} } // namespace exceptions } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a00d8f3b6b..af7f7c5804 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -37,8 +37,7 @@ using rclcpp::executor::ExecutorArgs; using rclcpp::executor::FutureReturnCode; Executor::Executor(const ExecutorArgs & args) -: spinning(false), - memory_strategy_(args.memory_strategy) +: spinning(false), memory_strategy_(args.memory_strategy) { rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); rcl_ret_t ret = rcl_guard_condition_init( @@ -60,20 +59,14 @@ Executor::Executor(const ExecutorArgs & args) // Store the context for later use. context_ = args.context; - ret = rcl_wait_set_init( - &wait_set_, - 0, 2, 0, 0, 0, 0, - context_->get_rcl_context().get(), - allocator); + ret = + rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, 0, context_->get_rcl_context().get(), allocator); if (RCL_RET_OK != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to create wait set: %s", rcl_get_error_string().str); + RCUTILS_LOG_ERROR_NAMED("rclcpp", "failed to create wait set: %s", rcl_get_error_string().str); rcl_reset_error(); if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); + "rclcpp", "failed to destroy guard condition: %s", rcl_get_error_string().str); rcl_reset_error(); } throw std::runtime_error("Failed to create wait set in Executor constructor"); @@ -98,16 +91,13 @@ Executor::~Executor() // Finalize the wait set. if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy wait set: %s", rcl_get_error_string().str); + RCUTILS_LOG_ERROR_NAMED("rclcpp", "failed to destroy wait set: %s", rcl_get_error_string().str); rcl_reset_error(); } // Finalize the interrupt guard condition. if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); + "rclcpp", "failed to destroy guard condition: %s", rcl_get_error_string().str); rcl_reset_error(); } // Remove and release the sigint guard condition @@ -115,8 +105,7 @@ Executor::~Executor() context_->release_interrupt_guard_condition(&wait_set_, std::nothrow); } -void -Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) +void Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { // If the node already has an executor std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); @@ -144,14 +133,13 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition()); } -void -Executor::add_node(std::shared_ptr node_ptr, bool notify) +void Executor::add_node(std::shared_ptr node_ptr, bool notify) { this->add_node(node_ptr->get_node_base_interface(), notify); } -void -Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) +void Executor::remove_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { bool node_removed = false; { @@ -183,16 +171,13 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition()); } -void -Executor::remove_node(std::shared_ptr node_ptr, bool notify) +void Executor::remove_node(std::shared_ptr node_ptr, bool notify) { this->remove_node(node_ptr->get_node_base_interface(), notify); } -void -Executor::spin_node_once_nanoseconds( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, - std::chrono::nanoseconds timeout) +void Executor::spin_node_once_nanoseconds( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, std::chrono::nanoseconds timeout) { this->add_node(node, false); // non-blocking = true @@ -200,40 +185,37 @@ Executor::spin_node_once_nanoseconds( this->remove_node(node, false); } -void -Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) +void Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) { this->add_node(node, false); spin_some(); this->remove_node(node, false); } -void -Executor::spin_node_some(std::shared_ptr node) +void Executor::spin_node_some(std::shared_ptr node) { this->spin_node_some(node->get_node_base_interface()); } -void -Executor::spin_some(std::chrono::nanoseconds max_duration) +void Executor::spin_some(std::chrono::nanoseconds max_duration) { auto start = std::chrono::steady_clock::now(); auto max_duration_not_elapsed = [max_duration, start]() { - if (std::chrono::nanoseconds(0) == max_duration) { - // told to spin forever if need be - return true; - } else if (std::chrono::steady_clock::now() - start < max_duration) { - // told to spin only for some maximum amount of time - return true; - } - // spun too long - return false; - }; + if (std::chrono::nanoseconds(0) == max_duration) { + // told to spin forever if need be + return true; + } else if (std::chrono::steady_clock::now() - start < max_duration) { + // told to spin only for some maximum amount of time + return true; + } + // spun too long + return false; + }; if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCLCPP_SCOPE_EXIT(this->spinning.store(false);); while (spinning.load() && max_duration_not_elapsed()) { AnyExecutable any_exec; if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) { @@ -244,21 +226,19 @@ Executor::spin_some(std::chrono::nanoseconds max_duration) } } -void -Executor::spin_once(std::chrono::nanoseconds timeout) +void Executor::spin_once(std::chrono::nanoseconds timeout) { if (spinning.exchange(true)) { throw std::runtime_error("spin_once() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCLCPP_SCOPE_EXIT(this->spinning.store(false);); AnyExecutable any_exec; if (get_next_executable(any_exec, timeout)) { execute_any_executable(any_exec); } } -void -Executor::cancel() +void Executor::cancel() { spinning.store(false); if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { @@ -266,8 +246,8 @@ Executor::cancel() } } -void -Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) +void Executor::set_memory_strategy( + rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) { if (memory_strategy == nullptr) { throw std::runtime_error("Received NULL memory strategy in executor."); @@ -275,8 +255,7 @@ Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_ = memory_strategy; } -void -Executor::execute_any_executable(AnyExecutable & any_exec) +void Executor::execute_any_executable(AnyExecutable & any_exec) { if (!spinning.load()) { return; @@ -308,9 +287,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } } -void -Executor::execute_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) +void Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) { rmw_message_info_t message_info; message_info.from_intra_process = false; @@ -318,8 +295,7 @@ Executor::execute_subscription( if (subscription->is_serialized()) { auto serialized_msg = subscription->create_serialized_message(); auto ret = rcl_take_serialized_message( - subscription->get_subscription_handle().get(), - serialized_msg.get(), &message_info, nullptr); + subscription->get_subscription_handle().get(), serialized_msg.get(), &message_info, nullptr); if (RCL_RET_OK == ret) { auto void_serialized_msg = std::static_pointer_cast(serialized_msg); subscription->handle_message(void_serialized_msg, message_info); @@ -327,39 +303,35 @@ Executor::execute_subscription( RCUTILS_LOG_ERROR_NAMED( "rclcpp", "take_serialized failed for subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); + subscription->get_topic_name(), + rcl_get_error_string().str); rcl_reset_error(); } subscription->return_serialized_message(serialized_msg); } else { std::shared_ptr message = subscription->create_message(); auto ret = rcl_take( - subscription->get_subscription_handle().get(), - message.get(), &message_info, nullptr); + subscription->get_subscription_handle().get(), message.get(), &message_info, nullptr); if (RCL_RET_OK == ret) { subscription->handle_message(message, message_info); } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "could not deserialize serialized message on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); + subscription->get_topic_name(), + rcl_get_error_string().str); rcl_reset_error(); } subscription->return_message(message); } } -void -Executor::execute_intra_process_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) +void Executor::execute_intra_process_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) { rcl_interfaces::msg::IntraProcessMessage ipm; rmw_message_info_t message_info; rcl_ret_t status = rcl_take( - subscription->get_intra_process_subscription_handle().get(), - &ipm, - &message_info, - nullptr); + subscription->get_intra_process_subscription_handle().get(), &ipm, &message_info, nullptr); if (status == RCL_RET_OK) { message_info.from_intra_process = true; @@ -368,62 +340,54 @@ Executor::execute_intra_process_subscription( RCUTILS_LOG_ERROR_NAMED( "rclcpp", "take failed for intra process subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); + subscription->get_topic_name(), + rcl_get_error_string().str); rcl_reset_error(); } } -void -Executor::execute_timer( - rclcpp::TimerBase::SharedPtr timer) +void Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer) { timer->execute_callback(); } -void -Executor::execute_service( - rclcpp::ServiceBase::SharedPtr service) +void Executor::execute_service(rclcpp::ServiceBase::SharedPtr service) { auto request_header = service->create_request_header(); std::shared_ptr request = service->create_request(); - rcl_ret_t status = rcl_take_request( - service->get_service_handle().get(), - request_header.get(), - request.get()); + rcl_ret_t status = + rcl_take_request(service->get_service_handle().get(), request_header.get(), request.get()); if (status == RCL_RET_OK) { service->handle_request(request_header, request); } else if (status != RCL_RET_SERVICE_TAKE_FAILED) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "take request failed for server of service '%s': %s", - service->get_service_name(), rcl_get_error_string().str); + service->get_service_name(), + rcl_get_error_string().str); rcl_reset_error(); } } -void -Executor::execute_client( - rclcpp::ClientBase::SharedPtr client) +void Executor::execute_client(rclcpp::ClientBase::SharedPtr client) { auto request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); - rcl_ret_t status = rcl_take_response( - client->get_client_handle().get(), - request_header.get(), - response.get()); + rcl_ret_t status = + rcl_take_response(client->get_client_handle().get(), request_header.get(), response.get()); if (status == RCL_RET_OK) { client->handle_response(request_header, response); } else if (status != RCL_RET_CLIENT_TAKE_FAILED) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "take response failed for client of service '%s': %s", - client->get_service_name(), rcl_get_error_string().str); + client->get_service_name(), + rcl_get_error_string().str); rcl_reset_error(); } } -void -Executor::wait_for_work(std::chrono::nanoseconds timeout) +void Executor::wait_for_work(std::chrono::nanoseconds timeout) { { std::unique_lock lock(memory_strategy_mutex_); @@ -454,13 +418,16 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) // The size of waitables are accounted for in size of the other entities rcl_ret_t ret = rcl_wait_set_resize( - &wait_set_, memory_strategy_->number_of_ready_subscriptions(), - memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), - memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), + &wait_set_, + memory_strategy_->number_of_ready_subscriptions(), + memory_strategy_->number_of_guard_conditions(), + memory_strategy_->number_of_ready_timers(), + memory_strategy_->number_of_ready_clients(), + memory_strategy_->number_of_ready_services(), memory_strategy_->number_of_ready_events()); if (RCL_RET_OK != ret) { throw std::runtime_error( - std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str); + std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str); } if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) { @@ -471,8 +438,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) rcl_wait(&wait_set_, std::chrono::duration_cast(timeout).count()); if (status == RCL_RET_WAIT_SET_EMPTY) { RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in rcl_wait(). This should never happen."); + "rclcpp", "empty wait set received in rcl_wait(). This should never happen."); } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { using rclcpp::exceptions::throw_from_rcl_error; throw_from_rcl_error(status, "rcl_wait() failed"); @@ -483,8 +449,8 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) memory_strategy_->remove_null_handles(&wait_set_); } -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group) +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr Executor::get_node_by_group( + rclcpp::callback_group::CallbackGroup::SharedPtr group) { if (!group) { return nullptr; @@ -504,8 +470,8 @@ Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr gro return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr -Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) +rclcpp::callback_group::CallbackGroup::SharedPtr Executor::get_group_by_timer( + rclcpp::TimerBase::SharedPtr timer) { for (auto & weak_node : weak_nodes_) { auto node = weak_node.lock(); @@ -517,8 +483,8 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) if (!group) { continue; } - auto timer_ref = group->find_timer_ptrs_if( - [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool { + auto timer_ref = + group->find_timer_ptrs_if([timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool { return timer_ptr == timer; }); if (timer_ref) { @@ -529,8 +495,7 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) return rclcpp::callback_group::CallbackGroup::SharedPtr(); } -bool -Executor::get_next_ready_executable(AnyExecutable & any_executable) +bool Executor::get_next_ready_executable(AnyExecutable & any_executable) { // Check the timers to see if there are any that are ready memory_strategy_->get_next_timer(any_executable, weak_nodes_); @@ -561,8 +526,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) return false; } -bool -Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout) +bool Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout) { bool success = false; // Check to see if there are any subscriptions or timers needing service @@ -586,8 +550,7 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos using callback_group::CallbackGroupType; if ( any_executable.callback_group && - any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) - { + any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) { // It should not have been taken otherwise assert(any_executable.callback_group->can_be_taken_from().load()); // Set to false to indicate something is being run from this group @@ -599,14 +562,13 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos return success; } -std::ostream & -rclcpp::executor::operator<<(std::ostream & os, const FutureReturnCode & future_return_code) +std::ostream & rclcpp::executor::operator<<( + std::ostream & os, const FutureReturnCode & future_return_code) { return os << to_string(future_return_code); } -std::string -rclcpp::executor::to_string(const FutureReturnCode & future_return_code) +std::string rclcpp::executor::to_string(const FutureReturnCode & future_return_code) { using enum_type = std::underlying_type::type; std::string prefix = "Unknown enum value ("; diff --git a/rclcpp/src/rclcpp/executors.cpp b/rclcpp/src/rclcpp/executors.cpp index 0a900c07da..9d8119a48f 100644 --- a/rclcpp/src/rclcpp/executors.cpp +++ b/rclcpp/src/rclcpp/executors.cpp @@ -14,21 +14,18 @@ #include "rclcpp/executors.hpp" -void -rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) +void rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { rclcpp::executors::SingleThreadedExecutor exec; exec.spin_node_some(node_ptr); } -void -rclcpp::spin_some(rclcpp::Node::SharedPtr node_ptr) +void rclcpp::spin_some(rclcpp::Node::SharedPtr node_ptr) { rclcpp::spin_some(node_ptr->get_node_base_interface()); } -void -rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) +void rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node_ptr); @@ -36,8 +33,7 @@ rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) exec.remove_node(node_ptr); } -void -rclcpp::spin(rclcpp::Node::SharedPtr node_ptr) +void rclcpp::spin(rclcpp::Node::SharedPtr node_ptr) { rclcpp::spin(node_ptr->get_node_base_interface()); } diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index e2f878abc7..dce200617a 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -19,8 +19,8 @@ #include #include -#include "rclcpp/utilities.hpp" #include "rclcpp/scope_exit.hpp" +#include "rclcpp/utilities.hpp" using rclcpp::executors::MultiThreadedExecutor; @@ -39,15 +39,16 @@ MultiThreadedExecutor::MultiThreadedExecutor( } } -MultiThreadedExecutor::~MultiThreadedExecutor() {} +MultiThreadedExecutor::~MultiThreadedExecutor() +{ +} -void -MultiThreadedExecutor::spin() +void MultiThreadedExecutor::spin() { if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCLCPP_SCOPE_EXIT(this->spinning.store(false);); std::vector threads; size_t thread_id = 0; { @@ -64,14 +65,12 @@ MultiThreadedExecutor::spin() } } -size_t -MultiThreadedExecutor::get_number_of_threads() +size_t MultiThreadedExecutor::get_number_of_threads() { return number_of_threads_; } -void -MultiThreadedExecutor::run(size_t) +void MultiThreadedExecutor::run(size_t) { while (rclcpp::ok(this->context_) && spinning.load()) { executor::AnyExecutable any_exec; diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 7f699d81ae..f62872c9ec 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -19,17 +19,20 @@ using rclcpp::executors::SingleThreadedExecutor; SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::executor::ExecutorArgs & args) -: executor::Executor(args) {} +: executor::Executor(args) +{ +} -SingleThreadedExecutor::~SingleThreadedExecutor() {} +SingleThreadedExecutor::~SingleThreadedExecutor() +{ +} -void -SingleThreadedExecutor::spin() +void SingleThreadedExecutor::spin() { if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCLCPP_SCOPE_EXIT(this->spinning.store(false);); while (rclcpp::ok(this->context_) && spinning.load()) { rclcpp::executor::AnyExecutable any_executable; if (get_next_executable(any_executable)) { diff --git a/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp b/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp index b6976e21c9..15de8b1359 100644 --- a/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp +++ b/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp @@ -23,14 +23,13 @@ #include "rcutils/logging_macros.h" #include "rcutils/types/string_map.h" #include "rmw/error_handling.h" +#include "rmw/validate_full_topic_name.h" #include "rmw/validate_namespace.h" #include "rmw/validate_node_name.h" -#include "rmw/validate_full_topic_name.h" using rclcpp::exceptions::throw_from_rcl_error; -std::string -rclcpp::expand_topic_or_service_name( +std::string rclcpp::expand_topic_or_service_name( const std::string & name, const std::string & node_name, const std::string & namespace_, @@ -120,19 +119,20 @@ rclcpp::expand_topic_or_service_name( if (rmw_ret != RMW_RET_OK) { if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { throw_from_rcl_error( - RCL_RET_INVALID_ARGUMENT, "failed to validate node name", - rmw_get_error_state(), rmw_reset_error); + RCL_RET_INVALID_ARGUMENT, + "failed to validate node name", + rmw_get_error_state(), + rmw_reset_error); } throw_from_rcl_error( - RCL_RET_ERROR, "failed to validate node name", - rmw_get_error_state(), rmw_reset_error); + RCL_RET_ERROR, "failed to validate node name", rmw_get_error_state(), rmw_reset_error); } if (validation_result != RMW_NODE_NAME_VALID) { throw rclcpp::exceptions::InvalidNodeNameError( - node_name.c_str(), - rmw_node_name_validation_result_string(validation_result), - invalid_index); + node_name.c_str(), + rmw_node_name_validation_result_string(validation_result), + invalid_index); } else { throw std::runtime_error("invalid rcl node name but valid rmw node name"); } @@ -147,19 +147,20 @@ rclcpp::expand_topic_or_service_name( if (rmw_ret != RMW_RET_OK) { if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { throw_from_rcl_error( - RCL_RET_INVALID_ARGUMENT, "failed to validate namespace", - rmw_get_error_state(), rmw_reset_error); + RCL_RET_INVALID_ARGUMENT, + "failed to validate namespace", + rmw_get_error_state(), + rmw_reset_error); } throw_from_rcl_error( - RCL_RET_ERROR, "failed to validate namespace", - rmw_get_error_state(), rmw_reset_error); + RCL_RET_ERROR, "failed to validate namespace", rmw_get_error_state(), rmw_reset_error); } if (validation_result != RMW_NAMESPACE_VALID) { throw rclcpp::exceptions::InvalidNamespaceError( - namespace_.c_str(), - rmw_namespace_validation_result_string(validation_result), - invalid_index); + namespace_.c_str(), + rmw_namespace_validation_result_string(validation_result), + invalid_index); } else { throw std::runtime_error("invalid rcl namespace but valid rmw namespace"); } @@ -177,25 +178,26 @@ rclcpp::expand_topic_or_service_name( if (rmw_ret != RMW_RET_OK) { if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { throw_from_rcl_error( - RCL_RET_INVALID_ARGUMENT, "failed to validate full topic name", - rmw_get_error_state(), rmw_reset_error); + RCL_RET_INVALID_ARGUMENT, + "failed to validate full topic name", + rmw_get_error_state(), + rmw_reset_error); } throw_from_rcl_error( - RCL_RET_ERROR, "failed to validate full topic name", - rmw_get_error_state(), rmw_reset_error); + RCL_RET_ERROR, "failed to validate full topic name", rmw_get_error_state(), rmw_reset_error); } if (validation_result != RMW_TOPIC_VALID) { if (is_service) { throw rclcpp::exceptions::InvalidServiceNameError( - result.c_str(), - rmw_full_topic_name_validation_result_string(validation_result), - invalid_index); + result.c_str(), + rmw_full_topic_name_validation_result_string(validation_result), + invalid_index); } else { throw rclcpp::exceptions::InvalidTopicNameError( - result.c_str(), - rmw_full_topic_name_validation_result_string(validation_result), - invalid_index); + result.c_str(), + rmw_full_topic_name_validation_result_string(validation_result), + invalid_index); } } diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index b097564333..1dd8878331 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -63,8 +63,7 @@ GraphListener::~GraphListener() this->shutdown(std::nothrow); } -void -GraphListener::start_if_not_started() +void GraphListener::start_if_not_started() { std::lock_guard shutdown_lock(shutdown_mutex_); if (is_shutdown_.load()) { @@ -89,22 +88,20 @@ GraphListener::start_if_not_started() // This is important to ensure that the wait set is finalized before // destruction of static objects occurs. std::weak_ptr weak_this = shared_from_this(); - rclcpp::on_shutdown( - [weak_this]() { - auto shared_this = weak_this.lock(); - if (shared_this) { - // should not throw from on_shutdown if it can be avoided - shared_this->shutdown(std::nothrow); - } - }); + rclcpp::on_shutdown([weak_this]() { + auto shared_this = weak_this.lock(); + if (shared_this) { + // should not throw from on_shutdown if it can be avoided + shared_this->shutdown(std::nothrow); + } + }); // Start the listener thread. listener_thread_ = std::thread(&GraphListener::run, this); is_started_ = true; } } -void -GraphListener::run() +void GraphListener::run() { try { run_loop(); @@ -116,15 +113,12 @@ GraphListener::run() exc.what()); std::rethrow_exception(std::current_exception()); } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "unknown error in GraphListener thread"); + RCUTILS_LOG_ERROR_NAMED("rclcpp", "unknown error in GraphListener thread"); std::rethrow_exception(std::current_exception()); } } -void -GraphListener::run_loop() +void GraphListener::run_loop() { while (true) { // If shutdown() was called, exit. @@ -217,8 +211,7 @@ GraphListener::run_loop() } // while (true) } -static void -interrupt_(rcl_guard_condition_t * interrupt_guard_condition) +static void interrupt_(rcl_guard_condition_t * interrupt_guard_condition) { rcl_ret_t ret = rcl_trigger_guard_condition(interrupt_guard_condition); if (RCL_RET_OK != ret) { @@ -226,8 +219,7 @@ interrupt_(rcl_guard_condition_t * interrupt_guard_condition) } } -static void -acquire_nodes_lock_( +static void acquire_nodes_lock_( std::mutex * node_graph_interfaces_barrier_mutex, std::mutex * node_graph_interfaces_mutex, rcl_guard_condition_t * interrupt_guard_condition) @@ -242,8 +234,7 @@ acquire_nodes_lock_( } } -static bool -has_node_( +static bool has_node_( std::vector * node_graph_interfaces, rclcpp::node_interfaces::NodeGraphInterface * node_graph) { @@ -255,8 +246,7 @@ has_node_( return false; } -bool -GraphListener::has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) +bool GraphListener::has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) { if (!node_graph) { return false; @@ -272,8 +262,7 @@ GraphListener::has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph return has_node_(&node_graph_interfaces_, node_graph); } -void -GraphListener::add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) +void GraphListener::add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) { if (!node_graph) { throw std::invalid_argument("node is nullptr"); @@ -299,8 +288,7 @@ GraphListener::add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph // will evaluate the new node when nodes_lock releases the node_graph_interfaces_mutex_. } -static void -remove_node_( +static void remove_node_( std::vector * node_graph_interfaces, rclcpp::node_interfaces::NodeGraphInterface * node_graph) { @@ -317,8 +305,7 @@ remove_node_( throw NodeNotFoundError(); } -void -GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) +void GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) { if (!node_graph) { throw std::invalid_argument("node is nullptr"); @@ -340,8 +327,7 @@ GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_gr remove_node_(&node_graph_interfaces_, node_graph); } -void -GraphListener::__shutdown(bool should_throw) +void GraphListener::__shutdown(bool should_throw) { std::lock_guard shutdown_lock(shutdown_mutex_); if (!is_shutdown_.exchange(true)) { @@ -371,14 +357,12 @@ GraphListener::__shutdown(bool should_throw) } } -void -GraphListener::shutdown() +void GraphListener::shutdown() { this->__shutdown(true); } -void -GraphListener::shutdown(const std::nothrow_t &) noexcept +void GraphListener::shutdown(const std::nothrow_t &) noexcept { try { this->__shutdown(false); @@ -386,16 +370,15 @@ GraphListener::shutdown(const std::nothrow_t &) noexcept RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), "caught %s exception when shutting down GraphListener: %s", - rmw::impl::cpp::demangle(exc).c_str(), exc.what()); + rmw::impl::cpp::demangle(exc).c_str(), + exc.what()); } catch (...) { RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "caught unknown exception when shutting down GraphListener"); + rclcpp::get_logger("rclcpp"), "caught unknown exception when shutting down GraphListener"); } } -bool -GraphListener::is_shutdown() +bool GraphListener::is_shutdown() { return is_shutdown_.load(); } diff --git a/rclcpp/src/rclcpp/init_options.cpp b/rclcpp/src/rclcpp/init_options.cpp index db753f6178..39032aad3c 100644 --- a/rclcpp/src/rclcpp/init_options.cpp +++ b/rclcpp/src/rclcpp/init_options.cpp @@ -20,8 +20,7 @@ namespace rclcpp { -InitOptions::InitOptions(rcl_allocator_t allocator) -: init_options_(new rcl_init_options_t) +InitOptions::InitOptions(rcl_allocator_t allocator) : init_options_(new rcl_init_options_t) { *init_options_ = rcl_get_zero_initialized_init_options(); rcl_ret_t ret = rcl_init_options_init(init_options_.get(), allocator); @@ -40,12 +39,11 @@ InitOptions::InitOptions(const rcl_init_options_t & init_options) } } -InitOptions::InitOptions(const InitOptions & other) -: InitOptions(*other.get_rcl_init_options()) -{} +InitOptions::InitOptions(const InitOptions & other) : InitOptions(*other.get_rcl_init_options()) +{ +} -InitOptions & -InitOptions::operator=(const InitOptions & other) +InitOptions & InitOptions::operator=(const InitOptions & other) { if (this != &other) { this->finalize_init_options(); @@ -62,23 +60,22 @@ InitOptions::~InitOptions() this->finalize_init_options(); } -void -InitOptions::finalize_init_options() +void InitOptions::finalize_init_options() { if (init_options_) { rcl_ret_t ret = rcl_init_options_fini(init_options_.get()); if (RCL_RET_OK != ret) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), - "failed to finalize rcl init options: %s", rcl_get_error_string().str); + "failed to finalize rcl init options: %s", + rcl_get_error_string().str); rcl_reset_error(); } *init_options_ = rcl_get_zero_initialized_init_options(); } } -const rcl_init_options_t * -InitOptions::get_rcl_init_options() const +const rcl_init_options_t * InitOptions::get_rcl_init_options() const { return init_options_.get(); } diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index 56e30ab3c0..d636890bfc 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -19,20 +19,20 @@ namespace rclcpp namespace intra_process_manager { -static std::atomic _next_unique_id {1}; +static std::atomic _next_unique_id{1}; IntraProcessManager::IntraProcessManager( rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr impl) : impl_(impl) -{} +{ +} IntraProcessManager::~IntraProcessManager() -{} +{ +} -uint64_t -IntraProcessManager::add_publisher( - rclcpp::PublisherBase::SharedPtr publisher, - size_t buffer_size) +uint64_t IntraProcessManager::add_publisher( + rclcpp::PublisherBase::SharedPtr publisher, size_t buffer_size) { auto id = IntraProcessManager::get_next_unique_id(); size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size(); @@ -44,41 +44,34 @@ IntraProcessManager::add_publisher( return id; } -uint64_t -IntraProcessManager::add_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) +uint64_t IntraProcessManager::add_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) { auto id = IntraProcessManager::get_next_unique_id(); impl_->add_subscription(id, subscription); return id; } -void -IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) +void IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) { impl_->remove_subscription(intra_process_subscription_id); } -void -IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id) +void IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id) { impl_->remove_publisher(intra_process_publisher_id); } -bool -IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const +bool IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const { return impl_->matches_any_publishers(id); } -size_t -IntraProcessManager::get_subscription_count(uint64_t intra_process_publisher_id) const +size_t IntraProcessManager::get_subscription_count(uint64_t intra_process_publisher_id) const { return impl_->get_subscription_count(intra_process_publisher_id); } -uint64_t -IntraProcessManager::get_next_unique_id() +uint64_t IntraProcessManager::get_next_unique_id() { auto next_id = _next_unique_id.fetch_add(1, std::memory_order_relaxed); // Check for rollover (we started at 1). diff --git a/rclcpp/src/rclcpp/logger.cpp b/rclcpp/src/rclcpp/logger.cpp index b3d25c038e..0c2c005004 100644 --- a/rclcpp/src/rclcpp/logger.cpp +++ b/rclcpp/src/rclcpp/logger.cpp @@ -21,8 +21,7 @@ namespace rclcpp { -Logger -get_logger(const std::string & name) +Logger get_logger(const std::string & name) { #if RCLCPP_LOGGING_ENABLED return rclcpp::Logger(name); @@ -32,8 +31,7 @@ get_logger(const std::string & name) #endif } -Logger -get_node_logger(const rcl_node_t * node) +Logger get_node_logger(const rcl_node_t * node) { const char * logger_name = rcl_node_get_logger_name(node); if (nullptr == logger_name) { diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index 26c3e3c98c..df43b1db6c 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -17,10 +17,8 @@ using rclcpp::memory_strategy::MemoryStrategy; -rclcpp::SubscriptionBase::SharedPtr -MemoryStrategy::get_subscription_by_handle( - std::shared_ptr subscriber_handle, - const WeakNodeList & weak_nodes) +rclcpp::SubscriptionBase::SharedPtr MemoryStrategy::get_subscription_by_handle( + std::shared_ptr subscriber_handle, const WeakNodeList & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -34,9 +32,8 @@ MemoryStrategy::get_subscription_by_handle( } auto match_subscription = group->find_subscription_ptrs_if( [&subscriber_handle](const rclcpp::SubscriptionBase::SharedPtr & subscription) -> bool { - return - (subscription->get_subscription_handle() == subscriber_handle) || - (subscription->get_intra_process_subscription_handle() == subscriber_handle); + return (subscription->get_subscription_handle() == subscriber_handle) || + (subscription->get_intra_process_subscription_handle() == subscriber_handle); }); if (match_subscription) { return match_subscription; @@ -46,10 +43,8 @@ MemoryStrategy::get_subscription_by_handle( return nullptr; } -rclcpp::ServiceBase::SharedPtr -MemoryStrategy::get_service_by_handle( - std::shared_ptr service_handle, - const WeakNodeList & weak_nodes) +rclcpp::ServiceBase::SharedPtr MemoryStrategy::get_service_by_handle( + std::shared_ptr service_handle, const WeakNodeList & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -73,10 +68,8 @@ MemoryStrategy::get_service_by_handle( return nullptr; } -rclcpp::ClientBase::SharedPtr -MemoryStrategy::get_client_by_handle( - std::shared_ptr client_handle, - const WeakNodeList & weak_nodes) +rclcpp::ClientBase::SharedPtr MemoryStrategy::get_client_by_handle( + std::shared_ptr client_handle, const WeakNodeList & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -100,10 +93,8 @@ MemoryStrategy::get_client_by_handle( return nullptr; } -rclcpp::TimerBase::SharedPtr -MemoryStrategy::get_timer_by_handle( - std::shared_ptr timer_handle, - const WeakNodeList & weak_nodes) +rclcpp::TimerBase::SharedPtr MemoryStrategy::get_timer_by_handle( + std::shared_ptr timer_handle, const WeakNodeList & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -127,10 +118,8 @@ MemoryStrategy::get_timer_by_handle( return nullptr; } -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -MemoryStrategy::get_node_by_group( - rclcpp::callback_group::CallbackGroup::SharedPtr group, - const WeakNodeList & weak_nodes) +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MemoryStrategy::get_node_by_group( + rclcpp::callback_group::CallbackGroup::SharedPtr group, const WeakNodeList & weak_nodes) { if (!group) { return nullptr; @@ -150,10 +139,8 @@ MemoryStrategy::get_node_by_group( return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr -MemoryStrategy::get_group_by_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription, - const WeakNodeList & weak_nodes) +rclcpp::callback_group::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription, const WeakNodeList & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -177,10 +164,8 @@ MemoryStrategy::get_group_by_subscription( return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr -MemoryStrategy::get_group_by_service( - rclcpp::ServiceBase::SharedPtr service, - const WeakNodeList & weak_nodes) +rclcpp::callback_group::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_service( + rclcpp::ServiceBase::SharedPtr service, const WeakNodeList & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -204,10 +189,8 @@ MemoryStrategy::get_group_by_service( return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr -MemoryStrategy::get_group_by_client( - rclcpp::ClientBase::SharedPtr client, - const WeakNodeList & weak_nodes) +rclcpp::callback_group::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_client( + rclcpp::ClientBase::SharedPtr client, const WeakNodeList & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -220,9 +203,7 @@ MemoryStrategy::get_group_by_client( continue; } auto client_ref = group->find_client_ptrs_if( - [&client](const rclcpp::ClientBase::SharedPtr & cli) -> bool { - return cli == client; - }); + [&client](const rclcpp::ClientBase::SharedPtr & cli) -> bool { return cli == client; }); if (client_ref) { return group; } @@ -231,10 +212,8 @@ MemoryStrategy::get_group_by_client( return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr -MemoryStrategy::get_group_by_timer( - rclcpp::TimerBase::SharedPtr timer, - const WeakNodeList & weak_nodes) +rclcpp::callback_group::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_timer( + rclcpp::TimerBase::SharedPtr timer, const WeakNodeList & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); @@ -247,9 +226,7 @@ MemoryStrategy::get_group_by_timer( continue; } auto timer_ref = group->find_timer_ptrs_if( - [&timer](const rclcpp::TimerBase::SharedPtr & time) -> bool { - return time == timer; - }); + [&timer](const rclcpp::TimerBase::SharedPtr & time) -> bool { return time == timer; }); if (timer_ref) { return group; } @@ -258,10 +235,8 @@ MemoryStrategy::get_group_by_timer( return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr -MemoryStrategy::get_group_by_waitable( - rclcpp::Waitable::SharedPtr waitable, - const WeakNodeList & weak_nodes) +rclcpp::callback_group::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_waitable( + rclcpp::Waitable::SharedPtr waitable, const WeakNodeList & weak_nodes) { for (auto & weak_node : weak_nodes) { auto node = weak_node.lock(); diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 783a7e106e..bd3bbbf949 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -41,8 +41,8 @@ using rclcpp::NodeOptions; using rclcpp::exceptions::throw_from_rcl_error; RCLCPP_LOCAL -std::string -extend_sub_namespace(const std::string & existing_sub_namespace, const std::string & extension) +std::string extend_sub_namespace( + const std::string & existing_sub_namespace, const std::string & extension) { // Assumption is that the existing_sub_namespace does not need checking // because it would be checked already when it was set with this function. @@ -50,10 +50,7 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri // check if the new sub-namespace extension is absolute if (extension.front() == '/') { throw rclcpp::exceptions::NameValidationError( - "sub_namespace", - extension.c_str(), - "a sub-namespace should not have a leading /", - 0); + "sub_namespace", extension.c_str(), "a sub-namespace should not have a leading /", 0); } std::string new_sub_namespace; @@ -72,8 +69,8 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri } RCLCPP_LOCAL -std::string -create_effective_namespace(const std::string & node_namespace, const std::string & sub_namespace) +std::string create_effective_namespace( + const std::string & node_namespace, const std::string & sub_namespace) { // Assumption is that both the node_namespace and sub_namespace are conforming // and do not need trimming of `/` and other things, as they were validated @@ -87,58 +84,47 @@ create_effective_namespace(const std::string & node_namespace, const std::string } } -Node::Node( - const std::string & node_name, - const NodeOptions & options) +Node::Node(const std::string & node_name, const NodeOptions & options) : Node(node_name, "", options) { } Node::Node( - const std::string & node_name, - const std::string & namespace_, - const NodeOptions & options) + const std::string & node_name, const std::string & namespace_, const NodeOptions & options) : node_base_(new rclcpp::node_interfaces::NodeBase( - node_name, - namespace_, - options.context(), - *(options.get_rcl_node_options()), - options.use_intra_process_comms())), + node_name, + namespace_, + options.context(), + *(options.get_rcl_node_options()), + options.use_intra_process_comms())), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), node_clock_(new rclcpp::node_interfaces::NodeClock( - node_base_, - node_topics_, - node_graph_, - node_services_, - node_logging_ - )), + node_base_, node_topics_, node_graph_, node_services_, node_logging_)), node_parameters_(new rclcpp::node_interfaces::NodeParameters( - node_base_, - node_logging_, - node_topics_, - node_services_, - node_clock_, - options.parameter_overrides(), - options.start_parameter_services(), - options.start_parameter_event_publisher(), - options.parameter_event_qos(), - options.parameter_event_publisher_options(), - options.allow_undeclared_parameters(), - options.automatically_declare_parameters_from_overrides() - )), + node_base_, + node_logging_, + node_topics_, + node_services_, + node_clock_, + options.parameter_overrides(), + options.start_parameter_services(), + options.start_parameter_event_publisher(), + options.parameter_event_qos(), + options.parameter_event_publisher_options(), + options.allow_undeclared_parameters(), + options.automatically_declare_parameters_from_overrides())), node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( - node_base_, - node_topics_, - node_graph_, - node_services_, - node_logging_, - node_clock_, - node_parameters_ - )), + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_, + node_clock_, + node_parameters_)), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), sub_namespace_(""), @@ -146,9 +132,7 @@ Node::Node( { } -Node::Node( - const Node & other, - const std::string & sub_namespace) +Node::Node(const Node & other, const std::string & sub_namespace) : node_base_(other.node_base_), node_graph_(other.node_graph_), node_logging_(other.node_logging_), @@ -176,117 +160,100 @@ Node::Node( if (validation_result != RMW_NAMESPACE_VALID) { throw rclcpp::exceptions::InvalidNamespaceError( - effective_namespace_.c_str(), - rmw_namespace_validation_result_string(validation_result), - invalid_index); + effective_namespace_.c_str(), + rmw_namespace_validation_result_string(validation_result), + invalid_index); } } Node::~Node() -{} +{ +} -const char * -Node::get_name() const +const char * Node::get_name() const { return node_base_->get_name(); } -const char * -Node::get_namespace() const +const char * Node::get_namespace() const { return node_base_->get_namespace(); } -const char * -Node::get_fully_qualified_name() const +const char * Node::get_fully_qualified_name() const { return node_base_->get_fully_qualified_name(); } -rclcpp::Logger -Node::get_logger() const +rclcpp::Logger Node::get_logger() const { return node_logging_->get_logger(); } -rclcpp::callback_group::CallbackGroup::SharedPtr -Node::create_callback_group( +rclcpp::callback_group::CallbackGroup::SharedPtr Node::create_callback_group( rclcpp::callback_group::CallbackGroupType group_type) { return node_base_->create_callback_group(group_type); } -bool -Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +bool Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) { return node_base_->callback_group_in_node(group); } -const rclcpp::ParameterValue & -Node::declare_parameter( +const rclcpp::ParameterValue & Node::declare_parameter( const std::string & name, const rclcpp::ParameterValue & default_value, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, bool ignore_override) { return this->node_parameters_->declare_parameter( - name, - default_value, - parameter_descriptor, - ignore_override); + name, default_value, parameter_descriptor, ignore_override); } -void -Node::undeclare_parameter(const std::string & name) +void Node::undeclare_parameter(const std::string & name) { this->node_parameters_->undeclare_parameter(name); } -bool -Node::has_parameter(const std::string & name) const +bool Node::has_parameter(const std::string & name) const { return this->node_parameters_->has_parameter(name); } -rcl_interfaces::msg::SetParametersResult -Node::set_parameter(const rclcpp::Parameter & parameter) +rcl_interfaces::msg::SetParametersResult Node::set_parameter(const rclcpp::Parameter & parameter) { return this->set_parameters_atomically({parameter}); } -std::vector -Node::set_parameters(const std::vector & parameters) +std::vector Node::set_parameters( + const std::vector & parameters) { return node_parameters_->set_parameters(parameters); } -rcl_interfaces::msg::SetParametersResult -Node::set_parameters_atomically(const std::vector & parameters) +rcl_interfaces::msg::SetParametersResult Node::set_parameters_atomically( + const std::vector & parameters) { return node_parameters_->set_parameters_atomically(parameters); } -rclcpp::Parameter -Node::get_parameter(const std::string & name) const +rclcpp::Parameter Node::get_parameter(const std::string & name) const { return node_parameters_->get_parameter(name); } -bool -Node::get_parameter(const std::string & name, rclcpp::Parameter & parameter) const +bool Node::get_parameter(const std::string & name, rclcpp::Parameter & parameter) const { return node_parameters_->get_parameter(name, parameter); } -std::vector -Node::get_parameters( - const std::vector & names) const +std::vector Node::get_parameters(const std::vector & names) const { return node_parameters_->get_parameters(names); } -rcl_interfaces::msg::ParameterDescriptor -Node::describe_parameter(const std::string & name) const +rcl_interfaces::msg::ParameterDescriptor Node::describe_parameter(const std::string & name) const { auto result = node_parameters_->describe_parameters({name}); if (0 == result.size()) { @@ -298,192 +265,164 @@ Node::describe_parameter(const std::string & name) const return result.front(); } -std::vector -Node::describe_parameters(const std::vector & names) const +std::vector Node::describe_parameters( + const std::vector & names) const { return node_parameters_->describe_parameters(names); } -std::vector -Node::get_parameter_types(const std::vector & names) const +std::vector Node::get_parameter_types(const std::vector & names) const { return node_parameters_->get_parameter_types(names); } -rcl_interfaces::msg::ListParametersResult -Node::list_parameters(const std::vector & prefixes, uint64_t depth) const +rcl_interfaces::msg::ListParametersResult Node::list_parameters( + const std::vector & prefixes, uint64_t depth) const { return node_parameters_->list_parameters(prefixes, depth); } -rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr -Node::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr Node::add_on_set_parameters_callback( + OnParametersSetCallbackType callback) { return node_parameters_->add_on_set_parameters_callback(callback); } -void -Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback) +void Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback) { return node_parameters_->remove_on_set_parameters_callback(callback); } -rclcpp::Node::OnParametersSetCallbackType -Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) +rclcpp::Node::OnParametersSetCallbackType Node::set_on_parameters_set_callback( + rclcpp::Node::OnParametersSetCallbackType callback) { return node_parameters_->set_on_parameters_set_callback(callback); } -std::vector -Node::get_node_names() const +std::vector Node::get_node_names() const { return node_graph_->get_node_names(); } -std::map> -Node::get_topic_names_and_types() const +std::map> Node::get_topic_names_and_types() const { return node_graph_->get_topic_names_and_types(); } -std::map> -Node::get_service_names_and_types() const +std::map> Node::get_service_names_and_types() const { return node_graph_->get_service_names_and_types(); } -size_t -Node::count_publishers(const std::string & topic_name) const +size_t Node::count_publishers(const std::string & topic_name) const { return node_graph_->count_publishers(topic_name); } -size_t -Node::count_subscribers(const std::string & topic_name) const +size_t Node::count_subscribers(const std::string & topic_name) const { return node_graph_->count_subscribers(topic_name); } -const std::vector & -Node::get_callback_groups() const +const std::vector & Node::get_callback_groups() + const { return node_base_->get_callback_groups(); } -rclcpp::Event::SharedPtr -Node::get_graph_event() +rclcpp::Event::SharedPtr Node::get_graph_event() { return node_graph_->get_graph_event(); } -void -Node::wait_for_graph_change( - rclcpp::Event::SharedPtr event, - std::chrono::nanoseconds timeout) +void Node::wait_for_graph_change(rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout) { node_graph_->wait_for_graph_change(event, timeout); } -rclcpp::Clock::SharedPtr -Node::get_clock() +rclcpp::Clock::SharedPtr Node::get_clock() { return node_clock_->get_clock(); } -rclcpp::Time -Node::now() +rclcpp::Time Node::now() { return node_clock_->get_clock()->now(); } -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -Node::get_node_base_interface() +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr Node::get_node_base_interface() { return node_base_; } -rclcpp::node_interfaces::NodeClockInterface::SharedPtr -Node::get_node_clock_interface() +rclcpp::node_interfaces::NodeClockInterface::SharedPtr Node::get_node_clock_interface() { return node_clock_; } -rclcpp::node_interfaces::NodeGraphInterface::SharedPtr -Node::get_node_graph_interface() +rclcpp::node_interfaces::NodeGraphInterface::SharedPtr Node::get_node_graph_interface() { return node_graph_; } -rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr -Node::get_node_logging_interface() +rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr Node::get_node_logging_interface() { return node_logging_; } -rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr -Node::get_node_time_source_interface() +rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr Node::get_node_time_source_interface() { return node_time_source_; } -rclcpp::node_interfaces::NodeTimersInterface::SharedPtr -Node::get_node_timers_interface() +rclcpp::node_interfaces::NodeTimersInterface::SharedPtr Node::get_node_timers_interface() { return node_timers_; } -rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr -Node::get_node_topics_interface() +rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr Node::get_node_topics_interface() { return node_topics_; } -rclcpp::node_interfaces::NodeServicesInterface::SharedPtr -Node::get_node_services_interface() +rclcpp::node_interfaces::NodeServicesInterface::SharedPtr Node::get_node_services_interface() { return node_services_; } -rclcpp::node_interfaces::NodeParametersInterface::SharedPtr -Node::get_node_parameters_interface() +rclcpp::node_interfaces::NodeParametersInterface::SharedPtr Node::get_node_parameters_interface() { return node_parameters_; } -rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr -Node::get_node_waitables_interface() +rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr Node::get_node_waitables_interface() { return node_waitables_; } -const std::string & -Node::get_sub_namespace() const +const std::string & Node::get_sub_namespace() const { return this->sub_namespace_; } -const std::string & -Node::get_effective_namespace() const +const std::string & Node::get_effective_namespace() const { return this->effective_namespace_; } -Node::SharedPtr -Node::create_sub_node(const std::string & sub_namespace) +Node::SharedPtr Node::create_sub_node(const std::string & sub_namespace) { // Cannot use make_shared() here as it requires the constructor to be // public, and this constructor is intentionally protected instead. return std::shared_ptr(new Node(*this, sub_namespace)); } -const NodeOptions & -Node::get_node_options() const +const NodeOptions & Node::get_node_options() const { return this->node_options_; } -bool -Node::assert_liveliness() const +bool Node::assert_liveliness() const { return node_base_->assert_liveliness(); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 9241adfd6a..3234c33c64 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include +#include #include #include "rclcpp/node_interfaces/node_base.hpp" @@ -52,21 +52,22 @@ NodeBase::NodeBase( // Setup a safe exit lamda to clean up the guard condition in case of an error here. auto finalize_notify_guard_condition = [this]() { - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - } - }; + // Finalize the interrupt guard condition. + if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", "failed to destroy guard condition: %s", rcl_get_error_string().str); + } + }; // Create the rcl node and store it in a shared_ptr with a custom destructor. std::unique_ptr rcl_node(new rcl_node_t(rcl_get_zero_initialized_node())); ret = rcl_node_init( rcl_node.get(), - node_name.c_str(), namespace_.c_str(), - context_->get_rcl_context().get(), &rcl_node_options); + node_name.c_str(), + namespace_.c_str(), + context_->get_rcl_context().get(), + &rcl_node_options); if (ret != RCL_RET_OK) { // Finalize the interrupt guard condition. finalize_notify_guard_condition(); @@ -86,9 +87,9 @@ NodeBase::NodeBase( if (validation_result != RMW_NODE_NAME_VALID) { throw rclcpp::exceptions::InvalidNodeNameError( - node_name.c_str(), - rmw_node_name_validation_result_string(validation_result), - invalid_index); + node_name.c_str(), + rmw_node_name_validation_result_string(validation_result), + invalid_index); } else { throw std::runtime_error("valid rmw node name but invalid rcl node name"); } @@ -109,9 +110,9 @@ NodeBase::NodeBase( if (validation_result != RMW_NAMESPACE_VALID) { throw rclcpp::exceptions::InvalidNamespaceError( - namespace_.c_str(), - rmw_namespace_validation_result_string(validation_result), - invalid_index); + namespace_.c_str(), + rmw_namespace_validation_result_string(validation_result), + invalid_index); } else { throw std::runtime_error("valid rmw node namespace but invalid rcl node namespace"); } @@ -119,16 +120,13 @@ NodeBase::NodeBase( throw_from_rcl_error(ret, "failed to initialize rcl node"); } - node_handle_.reset( - rcl_node.release(), - [](rcl_node_t * node) -> void { - if (rcl_node_fini(node) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl node handle: %s", rcl_get_error_string().str); - } - delete node; - }); + node_handle_.reset(rcl_node.release(), [](rcl_node_t * node) -> void { + if (rcl_node_fini(node) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", "Error in destruction of rcl node handle: %s", rcl_get_error_string().str); + } + delete node; + }); // Create the default callback group. using rclcpp::callback_group::CallbackGroupType; @@ -146,68 +144,58 @@ NodeBase::~NodeBase() notify_guard_condition_is_valid_ = false; if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); + "rclcpp", "failed to destroy guard condition: %s", rcl_get_error_string().str); } } } -const char * -NodeBase::get_name() const +const char * NodeBase::get_name() const { return rcl_node_get_name(node_handle_.get()); } -const char * -NodeBase::get_namespace() const +const char * NodeBase::get_namespace() const { return rcl_node_get_namespace(node_handle_.get()); } -const char * -NodeBase::get_fully_qualified_name() const +const char * NodeBase::get_fully_qualified_name() const { return rcl_node_get_fully_qualified_name(node_handle_.get()); } -rclcpp::Context::SharedPtr -NodeBase::get_context() +rclcpp::Context::SharedPtr NodeBase::get_context() { return context_; } -rcl_node_t * -NodeBase::get_rcl_node_handle() +rcl_node_t * NodeBase::get_rcl_node_handle() { return node_handle_.get(); } -const rcl_node_t * -NodeBase::get_rcl_node_handle() const +const rcl_node_t * NodeBase::get_rcl_node_handle() const { return node_handle_.get(); } -std::shared_ptr -NodeBase::get_shared_rcl_node_handle() +std::shared_ptr NodeBase::get_shared_rcl_node_handle() { return node_handle_; } -std::shared_ptr -NodeBase::get_shared_rcl_node_handle() const +std::shared_ptr NodeBase::get_shared_rcl_node_handle() const { return node_handle_; } -bool -NodeBase::assert_liveliness() const +bool NodeBase::assert_liveliness() const { return RCL_RET_OK == rcl_node_assert_liveliness(get_rcl_node_handle()); } -rclcpp::callback_group::CallbackGroup::SharedPtr -NodeBase::create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) +rclcpp::callback_group::CallbackGroup::SharedPtr NodeBase::create_callback_group( + rclcpp::callback_group::CallbackGroupType group_type) { using rclcpp::callback_group::CallbackGroup; using rclcpp::callback_group::CallbackGroupType; @@ -216,14 +204,12 @@ NodeBase::create_callback_group(rclcpp::callback_group::CallbackGroupType group_ return group; } -rclcpp::callback_group::CallbackGroup::SharedPtr -NodeBase::get_default_callback_group() +rclcpp::callback_group::CallbackGroup::SharedPtr NodeBase::get_default_callback_group() { return default_callback_group_; } -bool -NodeBase::callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +bool NodeBase::callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) { bool group_belongs_to_this_node = false; for (auto & weak_group : this->callback_groups_) { @@ -235,20 +221,18 @@ NodeBase::callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPt return group_belongs_to_this_node; } -const std::vector & -NodeBase::get_callback_groups() const +const std::vector & NodeBase::get_callback_groups() + const { return callback_groups_; } -std::atomic_bool & -NodeBase::get_associated_with_executor_atomic() +std::atomic_bool & NodeBase::get_associated_with_executor_atomic() { return associated_with_executor_; } -rcl_guard_condition_t * -NodeBase::get_notify_guard_condition() +rcl_guard_condition_t * NodeBase::get_notify_guard_condition() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); if (!notify_guard_condition_is_valid_) { @@ -257,14 +241,12 @@ NodeBase::get_notify_guard_condition() return ¬ify_guard_condition_; } -std::unique_lock -NodeBase::acquire_notify_guard_condition_lock() const +std::unique_lock NodeBase::acquire_notify_guard_condition_lock() const { return std::unique_lock(notify_guard_condition_mutex_); } -bool -NodeBase::get_use_intra_process_default() const +bool NodeBase::get_use_intra_process_default() const { return use_intra_process_default_; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp index 8de8c3cba5..9603b5351c 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp @@ -31,13 +31,14 @@ NodeClock::NodeClock( node_services_(node_services), node_logging_(node_logging), ros_clock_(std::make_shared(RCL_ROS_TIME)) -{} +{ +} NodeClock::~NodeClock() -{} +{ +} -std::shared_ptr -NodeClock::get_clock() +std::shared_ptr NodeClock::get_clock() { return ros_clock_; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 11347881b3..3b1ec96c9c 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -21,22 +21,22 @@ #include #include "rcl/graph.h" -#include "rclcpp/exceptions.hpp" #include "rclcpp/event.hpp" +#include "rclcpp/exceptions.hpp" #include "rclcpp/graph_listener.hpp" -using rclcpp::node_interfaces::NodeGraph; using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::graph_listener::GraphListener; +using rclcpp::node_interfaces::NodeGraph; NodeGraph::NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base) : node_base_(node_base), graph_listener_( - node_base->get_context()->get_sub_context(node_base->get_context()) - ), + node_base->get_context()->get_sub_context(node_base->get_context())), should_add_to_graph_listener_(true), graph_users_count_(0) -{} +{ +} NodeGraph::~NodeGraph() { @@ -49,24 +49,21 @@ NodeGraph::~NodeGraph() } } -std::map> -NodeGraph::get_topic_names_and_types(bool no_demangle) const +std::map> NodeGraph::get_topic_names_and_types( + bool no_demangle) const { rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); auto ret = rcl_get_topic_names_and_types( - node_base_->get_rcl_node_handle(), - &allocator, - no_demangle, - &topic_names_and_types); + node_base_->get_rcl_node_handle(), &allocator, no_demangle, &topic_names_and_types); if (ret != RCL_RET_OK) { - auto error_msg = std::string("failed to get topic names and types: ") + - rcl_get_error_string().str; + auto error_msg = + std::string("failed to get topic names and types: ") + rcl_get_error_string().str; rcl_reset_error(); if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) { error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") + - rcl_get_error_string().str; + rcl_get_error_string().str; } throw std::runtime_error(error_msg + rcl_get_error_string().str); } @@ -90,19 +87,16 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const return topics_and_types; } -std::map> -NodeGraph::get_service_names_and_types() const +std::map> NodeGraph::get_service_names_and_types() const { rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); auto ret = rcl_get_service_names_and_types( - node_base_->get_rcl_node_handle(), - &allocator, - &service_names_and_types); + node_base_->get_rcl_node_handle(), &allocator, &service_names_and_types); if (ret != RCL_RET_OK) { - auto error_msg = std::string("failed to get service names and types: ") + - rcl_get_error_string().str; + auto error_msg = + std::string("failed to get service names and types: ") + rcl_get_error_string().str; rcl_reset_error(); if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) { error_msg += @@ -131,8 +125,7 @@ NodeGraph::get_service_names_and_types() const return services_and_types; } -std::vector -NodeGraph::get_node_names() const +std::vector NodeGraph::get_node_names() const { std::vector nodes; auto names_and_namespaces = get_node_names_and_namespaces(); @@ -154,43 +147,35 @@ NodeGraph::get_node_names() const return_string = "/" + return_string; } return return_string; - } - ); + }); return nodes; } -std::vector> -NodeGraph::get_node_names_and_namespaces() const +std::vector> NodeGraph::get_node_names_and_namespaces() const { - rcutils_string_array_t node_names_c = - rcutils_get_zero_initialized_string_array(); - rcutils_string_array_t node_namespaces_c = - rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_names_c = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces_c = rcutils_get_zero_initialized_string_array(); auto allocator = rcl_get_default_allocator(); auto ret = rcl_get_node_names( - node_base_->get_rcl_node_handle(), - allocator, - &node_names_c, - &node_namespaces_c); + node_base_->get_rcl_node_handle(), allocator, &node_names_c, &node_namespaces_c); if (ret != RCL_RET_OK) { auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string().str; rcl_reset_error(); if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) { error_msg += std::string(", failed also to cleanup node names, leaking memory: ") + - rcl_get_error_string().str; + rcl_get_error_string().str; rcl_reset_error(); } if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) { error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") + - rcl_get_error_string().str; + rcl_get_error_string().str; rcl_reset_error(); } // TODO(karsten1987): Append rcutils_error_message once it's in master throw std::runtime_error(error_msg); } - std::vector> node_names; node_names.reserve(node_names_c.size); for (size_t i = 0; i < node_names_c.size; ++i) { @@ -222,8 +207,7 @@ NodeGraph::get_node_names_and_namespaces() const return node_names; } -size_t -NodeGraph::count_publishers(const std::string & topic_name) const +size_t NodeGraph::count_publishers(const std::string & topic_name) const { auto rcl_node_handle = node_base_->get_rcl_node_handle(); @@ -231,7 +215,7 @@ NodeGraph::count_publishers(const std::string & topic_name) const topic_name, rcl_node_get_name(rcl_node_handle), rcl_node_get_namespace(rcl_node_handle), - false); // false = not a service + false); // false = not a service size_t count; auto ret = rcl_count_publishers(rcl_node_handle, fqdn.c_str(), &count); @@ -244,8 +228,7 @@ NodeGraph::count_publishers(const std::string & topic_name) const return count; } -size_t -NodeGraph::count_subscribers(const std::string & topic_name) const +size_t NodeGraph::count_subscribers(const std::string & topic_name) const { auto rcl_node_handle = node_base_->get_rcl_node_handle(); @@ -253,7 +236,7 @@ NodeGraph::count_subscribers(const std::string & topic_name) const topic_name, rcl_node_get_name(rcl_node_handle), rcl_node_get_namespace(rcl_node_handle), - false); // false = not a service + false); // false = not a service size_t count; auto ret = rcl_count_subscribers(rcl_node_handle, fqdn.c_str(), &count); @@ -266,14 +249,12 @@ NodeGraph::count_subscribers(const std::string & topic_name) const return count; } -const rcl_guard_condition_t * -NodeGraph::get_graph_guard_condition() const +const rcl_guard_condition_t * NodeGraph::get_graph_guard_condition() const { return rcl_node_get_graph_guard_condition(node_base_->get_rcl_node_handle()); } -void -NodeGraph::notify_graph_change() +void NodeGraph::notify_graph_change() { { std::lock_guard graph_changed_lock(graph_mutex_); @@ -292,9 +273,7 @@ NodeGraph::notify_graph_change() std::remove_if( graph_events_.begin(), graph_events_.end(), - [](const rclcpp::Event::WeakPtr & wptr) { - return wptr.expired(); - }), + [](const rclcpp::Event::WeakPtr & wptr) { return wptr.expired(); }), graph_events_.end()); // update graph_users_count_ graph_users_count_.store(graph_events_.size()); @@ -310,15 +289,13 @@ NodeGraph::notify_graph_change() } } -void -NodeGraph::notify_shutdown() +void NodeGraph::notify_shutdown() { // notify here anything that will not be woken up by ctrl-c or rclcpp::shutdown(). graph_cv_.notify_all(); } -rclcpp::Event::SharedPtr -NodeGraph::get_graph_event() +rclcpp::Event::SharedPtr NodeGraph::get_graph_event() { auto event = rclcpp::Event::make_shared(); std::lock_guard graph_changed_lock(graph_mutex_); @@ -332,13 +309,11 @@ NodeGraph::get_graph_event() return event; } -void -NodeGraph::wait_for_graph_change( - rclcpp::Event::SharedPtr event, - std::chrono::nanoseconds timeout) +void NodeGraph::wait_for_graph_change( + rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout) { - using rclcpp::exceptions::InvalidEventError; using rclcpp::exceptions::EventNotRegisteredError; + using rclcpp::exceptions::InvalidEventError; if (!event) { throw InvalidEventError(); } @@ -356,16 +331,15 @@ NodeGraph::wait_for_graph_change( } } auto pred = [&event, context = node_base_->get_context()]() { - return event->check() || !rclcpp::ok(context); - }; + return event->check() || !rclcpp::ok(context); + }; std::unique_lock graph_lock(graph_mutex_); if (!pred()) { graph_cv_.wait_for(graph_lock, timeout, pred); } } -size_t -NodeGraph::count_graph_users() +size_t NodeGraph::count_graph_users() { return graph_users_count_.load(); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp b/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp index 269f14dac1..5445e06e17 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp @@ -26,14 +26,12 @@ NodeLogging::~NodeLogging() { } -rclcpp::Logger -NodeLogging::get_logger() const +rclcpp::Logger NodeLogging::get_logger() const { return logger_; } -const char * -NodeLogging::get_logger_name() const +const char * NodeLogging::get_logger_name() const { return rcl_node_get_logger_name(node_base_->get_rcl_node_handle()); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index c7d339932d..8bb3775cd4 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -68,10 +68,7 @@ NodeParameters::NodeParameters( if (start_parameter_event_publisher) { events_publisher_ = rclcpp::create_publisher( - node_topics, - "parameter_events", - parameter_event_qos, - publisher_options); + node_topics, "parameter_events", parameter_event_qos, publisher_options); } // Get the node options @@ -88,25 +85,24 @@ NodeParameters::NodeParameters( std::vector yaml_paths; auto get_yaml_paths = [&yaml_paths, &options](const rcl_arguments_t * args) { - int num_yaml_files = rcl_arguments_get_param_files_count(args); - if (num_yaml_files > 0) { - char ** param_files; - rcl_ret_t ret = rcl_arguments_get_param_files(args, options->allocator, ¶m_files); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - auto cleanup_param_files = make_scope_exit( - [¶m_files, &num_yaml_files, &options]() { - for (int i = 0; i < num_yaml_files; ++i) { - options->allocator.deallocate(param_files[i], options->allocator.state); - } - options->allocator.deallocate(param_files, options->allocator.state); - }); + int num_yaml_files = rcl_arguments_get_param_files_count(args); + if (num_yaml_files > 0) { + char ** param_files; + rcl_ret_t ret = rcl_arguments_get_param_files(args, options->allocator, ¶m_files); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + auto cleanup_param_files = make_scope_exit([¶m_files, &num_yaml_files, &options]() { for (int i = 0; i < num_yaml_files; ++i) { - yaml_paths.emplace_back(param_files[i]); + options->allocator.deallocate(param_files[i], options->allocator.state); } + options->allocator.deallocate(param_files, options->allocator.state); + }); + for (int i = 0; i < num_yaml_files; ++i) { + yaml_paths.emplace_back(param_files[i]); } - }; + } + }; // global before local so that local overwrites global if (options->use_global_arguments) { @@ -127,8 +123,8 @@ NodeParameters::NodeParameters( } if (!rcl_parse_yaml_file(yaml_path.c_str(), yaml_params)) { std::ostringstream ss; - ss << "Failed to parse parameters from file '" << yaml_path << "': " << - rcl_get_error_string().str; + ss << "Failed to parse parameters from file '" << yaml_path + << "': " << rcl_get_error_string().str; rcl_reset_error(); throw std::runtime_error(ss.str()); } @@ -150,8 +146,7 @@ NodeParameters::NodeParameters( // parameter overrides passed to constructor will overwrite overrides from yaml file sources for (auto & param : parameter_overrides) { - parameter_overrides_[param.get_name()] = - rclcpp::ParameterValue(param.get_value_message()); + parameter_overrides_[param.get_name()] = rclcpp::ParameterValue(param.get_value_message()); } // If asked, initialize any parameters that ended up in the initial parameter values, @@ -160,21 +155,18 @@ NodeParameters::NodeParameters( for (const auto & pair : this->get_parameter_overrides()) { if (!this->has_parameter(pair.first)) { this->declare_parameter( - pair.first, - pair.second, - rcl_interfaces::msg::ParameterDescriptor(), - true); + pair.first, pair.second, rcl_interfaces::msg::ParameterDescriptor(), true); } } } } NodeParameters::~NodeParameters() -{} +{ +} RCLCPP_LOCAL -bool -__lockless_has_parameter( +bool __lockless_has_parameter( const std::map & parameters, const std::string & name) { @@ -183,16 +175,13 @@ __lockless_has_parameter( // see https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon RCLCPP_LOCAL -bool -__are_doubles_equal(double x, double y, size_t ulp = 100) +bool __are_doubles_equal(double x, double y, size_t ulp = 100) { return std::abs(x - y) <= std::numeric_limits::epsilon() * std::abs(x + y) * ulp; } RCLCPP_LOCAL -inline -void -format_reason(std::string & reason, const std::string & name, const char * range_type) +inline void format_reason(std::string & reason, const std::string & name, const char * range_type) { std::ostringstream ss; ss << "Parameter {" << name << "} doesn't comply with " << range_type << " range."; @@ -200,10 +189,8 @@ format_reason(std::string & reason, const std::string & name, const char * range } RCLCPP_LOCAL -rcl_interfaces::msg::SetParametersResult -__check_parameter_value_in_range( - const rcl_interfaces::msg::ParameterDescriptor & descriptor, - const rclcpp::ParameterValue & value) +rcl_interfaces::msg::SetParametersResult __check_parameter_value_in_range( + const rcl_interfaces::msg::ParameterDescriptor & descriptor, const rclcpp::ParameterValue & value) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -256,8 +243,7 @@ __check_parameter_value_in_range( // Return true if parameter values comply with the descriptors in parameter_infos. RCLCPP_LOCAL -rcl_interfaces::msg::SetParametersResult -__check_parameters( +rcl_interfaces::msg::SetParametersResult __check_parameters( std::map & parameter_infos, const std::vector & parameters) { @@ -266,9 +252,7 @@ __check_parameters( for (const rclcpp::Parameter & parameter : parameters) { const rcl_interfaces::msg::ParameterDescriptor & descriptor = parameter_infos[parameter.get_name()].descriptor; - result = __check_parameter_value_in_range( - descriptor, - parameter.get_parameter_value()); + result = __check_parameter_value_in_range(descriptor, parameter.get_parameter_value()); if (!result.successful) { break; } @@ -278,14 +262,11 @@ __check_parameters( using OnParametersSetCallbackType = rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; -using CallbacksContainerType = - rclcpp::node_interfaces::NodeParameters::CallbacksContainerType; -using OnSetParametersCallbackHandle = - rclcpp::node_interfaces::OnSetParametersCallbackHandle; +using CallbacksContainerType = rclcpp::node_interfaces::NodeParameters::CallbacksContainerType; +using OnSetParametersCallbackHandle = rclcpp::node_interfaces::OnSetParametersCallbackHandle; RCLCPP_LOCAL -rcl_interfaces::msg::SetParametersResult -__call_on_parameters_set_callbacks( +rcl_interfaces::msg::SetParametersResult __call_on_parameters_set_callbacks( const std::vector & parameters, CallbacksContainerType & callback_container, const OnParametersSetCallbackType & callback) @@ -312,8 +293,7 @@ __call_on_parameters_set_callbacks( } RCLCPP_LOCAL -rcl_interfaces::msg::SetParametersResult -__set_parameters_atomically_common( +rcl_interfaces::msg::SetParametersResult __set_parameters_atomically_common( const std::vector & parameters, std::map & parameter_infos, CallbacksContainerType & callback_container, @@ -345,8 +325,7 @@ __set_parameters_atomically_common( } RCLCPP_LOCAL -rcl_interfaces::msg::SetParametersResult -__declare_parameter_common( +rcl_interfaces::msg::SetParametersResult __declare_parameter_common( const std::string & name, const rclcpp::ParameterValue & default_value, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, @@ -358,7 +337,7 @@ __declare_parameter_common( bool ignore_override = false) { using rclcpp::node_interfaces::ParameterInfo; - std::map parameter_infos {{name, ParameterInfo()}}; + std::map parameter_infos{{name, ParameterInfo()}}; parameter_infos.at(name).descriptor = parameter_descriptor; // Use the value from the overrides if available, otherwise use the default. @@ -369,13 +348,10 @@ __declare_parameter_common( } // Check with the user's callback to see if the initial value can be set. - std::vector parameter_wrappers {rclcpp::Parameter(name, *initial_value)}; + std::vector parameter_wrappers{rclcpp::Parameter(name, *initial_value)}; // This function also takes care of default vs initial value. auto result = __set_parameters_atomically_common( - parameter_wrappers, - parameter_infos, - callback_container, - callback); + parameter_wrappers, parameter_infos, callback_container, callback); // Add declared parameters to storage. parameters_out[name] = parameter_infos.at(name); @@ -388,8 +364,7 @@ __declare_parameter_common( return result; } -const rclcpp::ParameterValue & -NodeParameters::declare_parameter( +const rclcpp::ParameterValue & NodeParameters::declare_parameter( const std::string & name, const rclcpp::ParameterValue & default_value, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, @@ -407,7 +382,7 @@ NodeParameters::declare_parameter( // Error if this parameter has already been declared and is different if (__lockless_has_parameter(parameters_, name)) { throw rclcpp::exceptions::ParameterAlreadyDeclaredException( - "parameter '" + name + "' has already been declared"); + "parameter '" + name + "' has already been declared"); } rcl_interfaces::msg::ParameterEvent parameter_event; @@ -425,7 +400,7 @@ NodeParameters::declare_parameter( // If it failed to be set, then throw an exception. if (!result.successful) { throw rclcpp::exceptions::InvalidParameterValueException( - "parameter '" + name + "' could not be set: " + result.reason); + "parameter '" + name + "' could not be set: " + result.reason); } // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor. @@ -436,8 +411,7 @@ NodeParameters::declare_parameter( return parameters_.at(name).value; } -void -NodeParameters::undeclare_parameter(const std::string & name) +void NodeParameters::undeclare_parameter(const std::string & name) { std::lock_guard lock(mutex_); @@ -446,27 +420,26 @@ NodeParameters::undeclare_parameter(const std::string & name) auto parameter_info = parameters_.find(name); if (parameter_info == parameters_.end()) { throw rclcpp::exceptions::ParameterNotDeclaredException( - "cannot undeclare parameter '" + name + "' which has not yet been declared"); + "cannot undeclare parameter '" + name + "' which has not yet been declared"); } if (parameter_info->second.descriptor.read_only) { throw rclcpp::exceptions::ParameterImmutableException( - "cannot undeclare parameter '" + name + "' because it is read-only"); + "cannot undeclare parameter '" + name + "' because it is read-only"); } parameters_.erase(parameter_info); } -bool -NodeParameters::has_parameter(const std::string & name) const +bool NodeParameters::has_parameter(const std::string & name) const { std::lock_guard lock(mutex_); return __lockless_has_parameter(parameters_, name); } -std::vector -NodeParameters::set_parameters(const std::vector & parameters) +std::vector NodeParameters::set_parameters( + const std::vector & parameters) { std::vector results; results.reserve(parameters.size()); @@ -480,19 +453,15 @@ NodeParameters::set_parameters(const std::vector & parameters } template -auto -__find_parameter_by_name( - ParameterVectorType & parameters, - const std::string & name) +auto __find_parameter_by_name(ParameterVectorType & parameters, const std::string & name) { - return std::find_if( - parameters.begin(), - parameters.end(), - [&](auto parameter) {return parameter.get_name() == name;}); + return std::find_if(parameters.begin(), parameters.end(), [&](auto parameter) { + return parameter.get_name() == name; + }); } -rcl_interfaces::msg::SetParametersResult -NodeParameters::set_parameters_atomically(const std::vector & parameters) +rcl_interfaces::msg::SetParametersResult NodeParameters::set_parameters_atomically( + const std::vector & parameters) { std::lock_guard lock(mutex_); @@ -526,7 +495,7 @@ NodeParameters::set_parameters_atomically(const std::vector & } else { // If not, then throw the exception as documented. throw rclcpp::exceptions::ParameterNotDeclaredException( - "parameter '" + name + "' cannot be set because it was not declared"); + "parameter '" + name + "' cannot be set because it was not declared"); } } @@ -557,7 +526,7 @@ NodeParameters::set_parameters_atomically(const std::vector & parameter_overrides_, // Only call callbacks once below empty_callback_container, // callback_container is explicitly empty - nullptr, // callback is explicitly null. + nullptr, // callback is explicitly null. ¶meter_event_msg, true); if (!result.successful) { @@ -657,7 +626,7 @@ NodeParameters::set_parameters_atomically(const std::vector & auto it = std::find_if( parameters_to_be_undeclared.begin(), parameters_to_be_undeclared.end(), - [¶meter](const auto & p) {return p->get_name() == parameter.get_name();}); + [¶meter](const auto & p) { return p->get_name() == parameter.get_name(); }); if (it != parameters_to_be_undeclared.end()) { // This parameter was undeclared (deleted). continue; @@ -675,8 +644,8 @@ NodeParameters::set_parameters_atomically(const std::vector & return result; } -std::vector -NodeParameters::get_parameters(const std::vector & names) const +std::vector NodeParameters::get_parameters( + const std::vector & names) const { std::lock_guard lock(mutex_); std::vector results; @@ -698,8 +667,7 @@ NodeParameters::get_parameters(const std::vector & names) const return results; } -rclcpp::Parameter -NodeParameters::get_parameter(const std::string & name) const +rclcpp::Parameter NodeParameters::get_parameter(const std::string & name) const { rclcpp::Parameter parameter; @@ -712,18 +680,14 @@ NodeParameters::get_parameter(const std::string & name) const } } -bool -NodeParameters::get_parameter( - const std::string & name, - rclcpp::Parameter & parameter) const +bool NodeParameters::get_parameter(const std::string & name, rclcpp::Parameter & parameter) const { std::lock_guard lock(mutex_); auto param_iter = parameters_.find(name); if ( parameters_.end() != param_iter && - param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) - { + param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { parameter = {name, param_iter->second.value}; return true; } else { @@ -731,10 +695,8 @@ NodeParameters::get_parameter( } } -bool -NodeParameters::get_parameters_by_prefix( - const std::string & prefix, - std::map & parameters) const +bool NodeParameters::get_parameters_by_prefix( + const std::string & prefix, std::map & parameters) const { std::lock_guard lock(mutex_); @@ -752,8 +714,8 @@ NodeParameters::get_parameters_by_prefix( return ret; } -std::vector -NodeParameters::describe_parameters(const std::vector & names) const +std::vector NodeParameters::describe_parameters( + const std::vector & names) const { std::lock_guard lock(mutex_); std::vector results; @@ -780,8 +742,8 @@ NodeParameters::describe_parameters(const std::vector & names) cons return results; } -std::vector -NodeParameters::get_parameter_types(const std::vector & names) const +std::vector NodeParameters::get_parameter_types( + const std::vector & names) const { std::lock_guard lock(mutex_); std::vector results; @@ -806,8 +768,8 @@ NodeParameters::get_parameter_types(const std::vector & names) cons return results; } -rcl_interfaces::msg::ListParametersResult -NodeParameters::list_parameters(const std::vector & prefixes, uint64_t depth) const +rcl_interfaces::msg::ListParametersResult NodeParameters::list_parameters( + const std::vector & prefixes, uint64_t depth) const { std::lock_guard lock(mutex_); rcl_interfaces::msg::ListParametersResult result; @@ -816,12 +778,12 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 // using "." for now const char * separator = "."; for (auto & kv : parameters_) { - bool get_all = (prefixes.size() == 0) && + bool get_all = + (prefixes.size() == 0) && ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || - (static_cast(std::count(kv.first.begin(), kv.first.end(), *separator)) < depth)); + (static_cast(std::count(kv.first.begin(), kv.first.end(), *separator)) < depth)); bool prefix_matches = std::any_of( - prefixes.cbegin(), prefixes.cend(), - [&kv, &depth, &separator](const std::string & prefix) { + prefixes.cbegin(), prefixes.cend(), [&kv, &depth, &separator](const std::string & prefix) { if (kv.first == prefix) { return true; } else if (kv.first.find(prefix + separator) == 0) { @@ -829,7 +791,8 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 std::string substr = kv.first.substr(length); // Cast as unsigned integer to avoid warning return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || - (static_cast(std::count(substr.begin(), substr.end(), *separator)) < depth); + (static_cast(std::count(substr.begin(), substr.end(), *separator)) < + depth); } return false; }); @@ -840,8 +803,7 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 std::string prefix = kv.first.substr(0, last_separator); if ( std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == - result.prefixes.cend()) - { + result.prefixes.cend()) { result.prefixes.push_back(prefix); } } @@ -850,11 +812,11 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 return result; } -struct HandleCompare - : public std::unary_function +struct HandleCompare : public std::unary_function { - explicit HandleCompare(const OnSetParametersCallbackHandle * const base) - : base_(base) {} + explicit HandleCompare(const OnSetParametersCallbackHandle * const base) : base_(base) + { + } bool operator()(const OnSetParametersCallbackHandle::WeakPtr & handle) { auto shared_handle = handle.lock(); @@ -866,8 +828,7 @@ struct HandleCompare const OnSetParametersCallbackHandle * const base_; }; -void -NodeParameters::remove_on_set_parameters_callback( +void NodeParameters::remove_on_set_parameters_callback( const OnSetParametersCallbackHandle * const handle) { std::lock_guard lock(mutex_); @@ -884,8 +845,8 @@ NodeParameters::remove_on_set_parameters_callback( } } -OnSetParametersCallbackHandle::SharedPtr -NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +OnSetParametersCallbackHandle::SharedPtr NodeParameters::add_on_set_parameters_callback( + OnParametersSetCallbackType callback) { std::lock_guard lock(mutex_); ParameterMutationRecursionGuard guard(parameter_modification_enabled_); @@ -897,8 +858,8 @@ NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callb return handle; } -NodeParameters::OnParametersSetCallbackType -NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback) +NodeParameters::OnParametersSetCallbackType NodeParameters::set_on_parameters_set_callback( + OnParametersSetCallbackType callback) { std::lock_guard lock(mutex_); @@ -909,8 +870,8 @@ NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callb return existing_callback; } -const std::map & -NodeParameters::get_parameter_overrides() const +const std::map & NodeParameters::get_parameter_overrides() + const { return parameter_overrides_; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index 0e74f58907..bd74c21ed0 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -20,13 +20,14 @@ using rclcpp::node_interfaces::NodeServices; NodeServices::NodeServices(rclcpp::node_interfaces::NodeBaseInterface * node_base) : node_base_(node_base) -{} +{ +} NodeServices::~NodeServices() -{} +{ +} -void -NodeServices::add_service( +void NodeServices::add_service( rclcpp::ServiceBase::SharedPtr service_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group) { @@ -45,15 +46,13 @@ NodeServices::add_service( auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Failed to notify wait set on service creation: ") + - rmw_get_error_string().str - ); + std::string("Failed to notify wait set on service creation: ") + + rmw_get_error_string().str); } } } -void -NodeServices::add_client( +void NodeServices::add_client( rclcpp::ClientBase::SharedPtr client_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group) { @@ -72,9 +71,7 @@ NodeServices::add_client( auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Failed to notify wait set on client creation: ") + - rmw_get_error_string().str - ); + std::string("Failed to notify wait set on client creation: ") + rmw_get_error_string().str); } } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp b/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp index 7c678b7324..2e0e0411e4 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp @@ -47,4 +47,5 @@ NodeTimeSource::NodeTimeSource( } NodeTimeSource::~NodeTimeSource() -{} +{ +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index f9d5d720e8..a9b39a0e8e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -20,13 +20,14 @@ using rclcpp::node_interfaces::NodeTimers; NodeTimers::NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base) : node_base_(node_base) -{} +{ +} NodeTimers::~NodeTimers() -{} +{ +} -void -NodeTimers::add_timer( +void NodeTimers::add_timer( rclcpp::TimerBase::SharedPtr timer, rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) { @@ -41,7 +42,6 @@ NodeTimers::add_timer( } if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Failed to notify wait set on timer creation: ") + - rmw_get_error_string().str); + std::string("Failed to notify wait set on timer creation: ") + rmw_get_error_string().str); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 8bed67236f..d43c78f994 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -16,8 +16,8 @@ #include -#include "rclcpp/intra_process_manager.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/intra_process_manager.hpp" using rclcpp::exceptions::throw_from_rcl_error; @@ -25,21 +25,22 @@ using rclcpp::node_interfaces::NodeTopics; NodeTopics::NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base) : node_base_(node_base) -{} +{ +} NodeTopics::~NodeTopics() -{} +{ +} -rclcpp::PublisherBase::SharedPtr -NodeTopics::create_publisher( +rclcpp::PublisherBase::SharedPtr NodeTopics::create_publisher( const std::string & topic_name, const rclcpp::PublisherFactory & publisher_factory, const rcl_publisher_options_t & publisher_options, bool use_intra_process) { // Create the MessageT specific Publisher using the factory, but store it as PublisherBase. - auto publisher = publisher_factory.create_typed_publisher( - node_base_, topic_name, publisher_options); + auto publisher = + publisher_factory.create_typed_publisher(node_base_, topic_name, publisher_options); // Setup intra process publishing if requested. if (use_intra_process) { @@ -49,25 +50,21 @@ NodeTopics::create_publisher( // Register the publisher with the intra process manager. if (publisher_options.qos.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) { throw std::invalid_argument( - "intraprocess communication is not allowed with keep all history qos policy"); + "intraprocess communication is not allowed with keep all history qos policy"); } if (publisher_options.qos.depth == 0) { throw std::invalid_argument( - "intraprocess communication is not allowed with a zero qos history depth value"); + "intraprocess communication is not allowed with a zero qos history depth value"); } uint64_t intra_process_publisher_id = ipm->add_publisher(publisher); - publisher->setup_intra_process( - intra_process_publisher_id, - ipm, - publisher_options); + publisher->setup_intra_process(intra_process_publisher_id, ipm, publisher_options); } // Return the completed publisher. return publisher; } -void -NodeTopics::add_publisher( +void NodeTopics::add_publisher( rclcpp::PublisherBase::SharedPtr publisher, rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) { @@ -89,27 +86,25 @@ NodeTopics::add_publisher( auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Failed to notify wait set on publisher creation: ") + - rmw_get_error_string().str); + std::string("Failed to notify wait set on publisher creation: ") + + rmw_get_error_string().str); } } } -rclcpp::SubscriptionBase::SharedPtr -NodeTopics::create_subscription( +rclcpp::SubscriptionBase::SharedPtr NodeTopics::create_subscription( const std::string & topic_name, const rclcpp::SubscriptionFactory & subscription_factory, const rcl_subscription_options_t & subscription_options, bool use_intra_process) { - auto subscription = subscription_factory.create_typed_subscription( - node_base_, topic_name, subscription_options); + auto subscription = + subscription_factory.create_typed_subscription(node_base_, topic_name, subscription_options); // Setup intra process publishing if requested. if (use_intra_process) { auto context = node_base_->get_context(); - auto ipm = - context->get_sub_context(); + auto ipm = context->get_sub_context(); uint64_t intra_process_subscription_id = ipm->add_subscription(subscription); auto options_copy = subscription_options; options_copy.ignore_local_publications = false; @@ -120,8 +115,7 @@ NodeTopics::create_subscription( return subscription; } -void -NodeTopics::add_subscription( +void NodeTopics::add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) { @@ -145,15 +139,13 @@ NodeTopics::add_subscription( auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Failed to notify wait set on subscription creation: ") + - rmw_get_error_string().str - ); + std::string("Failed to notify wait set on subscription creation: ") + + rmw_get_error_string().str); } } } -rclcpp::node_interfaces::NodeBaseInterface * -NodeTopics::get_node_base_interface() const +rclcpp::node_interfaces::NodeBaseInterface * NodeTopics::get_node_base_interface() const { return node_base_; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index ee9a2da2de..e0926af812 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -20,15 +20,15 @@ using rclcpp::node_interfaces::NodeWaitables; NodeWaitables::NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base) : node_base_(node_base) -{} +{ +} NodeWaitables::~NodeWaitables() -{} +{ +} -void -NodeWaitables::add_waitable( - rclcpp::Waitable::SharedPtr waitable_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) +void NodeWaitables::add_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group) { if (group) { if (!node_base_->callback_group_in_node(group)) { @@ -45,15 +45,13 @@ NodeWaitables::add_waitable( auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { throw std::runtime_error( - std::string("Failed to notify wait set on waitable creation: ") + - rmw_get_error_string().str - ); + std::string("Failed to notify wait set on waitable creation: ") + + rmw_get_error_string().str); } } } -void -NodeWaitables::remove_waitable( +void NodeWaitables::remove_waitable( rclcpp::Waitable::SharedPtr waitable_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept { diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 65fe3221c9..626ce82b5e 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -17,8 +17,8 @@ #include #include #include -#include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/logging.hpp" @@ -32,9 +32,7 @@ namespace rclcpp namespace detail { -static -void -rcl_node_options_t_destructor(rcl_node_options_t * node_options) +static void rcl_node_options_t_destructor(rcl_node_options_t * node_options) { if (node_options) { rcl_ret_t ret = rcl_node_options_fini(node_options); @@ -42,7 +40,8 @@ rcl_node_options_t_destructor(rcl_node_options_t * node_options) // Cannot throw here, as it may be called in the destructor. RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), - "failed to finalize rcl node options: %s", rcl_get_error_string().str); + "failed to finalize rcl node options: %s", + rcl_get_error_string().str); rcl_reset_error(); } @@ -54,7 +53,8 @@ rcl_node_options_t_destructor(rcl_node_options_t * node_options) NodeOptions::NodeOptions(rcl_allocator_t allocator) : node_options_(nullptr, detail::rcl_node_options_t_destructor), allocator_(allocator) -{} +{ +} NodeOptions::NodeOptions(const NodeOptions & other) : node_options_(nullptr, detail::rcl_node_options_t_destructor) @@ -62,8 +62,7 @@ NodeOptions::NodeOptions(const NodeOptions & other) *this = other; } -NodeOptions & -NodeOptions::operator=(const NodeOptions & other) +NodeOptions & NodeOptions::operator=(const NodeOptions & other) { if (this != &other) { this->context_ = other.context_; @@ -80,8 +79,7 @@ NodeOptions::operator=(const NodeOptions & other) return *this; } -const rcl_node_options_t * -NodeOptions::get_rcl_node_options() const +const rcl_node_options_t * NodeOptions::get_rcl_node_options() const { // If it is nullptr, create it on demand. if (!node_options_) { @@ -92,7 +90,7 @@ NodeOptions::get_rcl_node_options() const node_options_->domain_id = this->get_domain_id_from_env(); int c_argc = 0; - std::unique_ptr c_argv; + std::unique_ptr c_argv; if (!this->arguments_.empty()) { if (this->arguments_.size() > static_cast(std::numeric_limits::max())) { throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args"); @@ -106,15 +104,14 @@ NodeOptions::get_rcl_node_options() const } } - rcl_ret_t ret = rcl_parse_arguments( - c_argc, c_argv.get(), this->allocator_, &(node_options_->arguments)); + rcl_ret_t ret = + rcl_parse_arguments(c_argc, c_argv.get(), this->allocator_, &(node_options_->arguments)); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "failed to parse arguments"); } - int unparsed_ros_args_count = - rcl_arguments_get_count_unparsed_ros(&(node_options_->arguments)); + int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(&(node_options_->arguments)); if (unparsed_ros_args_count > 0) { int * unparsed_ros_args_indices = nullptr; ret = rcl_arguments_get_unparsed_ros( @@ -138,153 +135,131 @@ NodeOptions::get_rcl_node_options() const return node_options_.get(); } -rclcpp::Context::SharedPtr -NodeOptions::context() const +rclcpp::Context::SharedPtr NodeOptions::context() const { return this->context_; } -NodeOptions & -NodeOptions::context(rclcpp::Context::SharedPtr context) +NodeOptions & NodeOptions::context(rclcpp::Context::SharedPtr context) { this->context_ = context; return *this; } -const std::vector & -NodeOptions::arguments() const +const std::vector & NodeOptions::arguments() const { return this->arguments_; } -NodeOptions & -NodeOptions::arguments(const std::vector & arguments) +NodeOptions & NodeOptions::arguments(const std::vector & arguments) { this->node_options_.reset(); // reset node options to make it be recreated on next access. this->arguments_ = arguments; return *this; } -std::vector & -NodeOptions::parameter_overrides() +std::vector & NodeOptions::parameter_overrides() { return this->parameter_overrides_; } -const std::vector & -NodeOptions::parameter_overrides() const +const std::vector & NodeOptions::parameter_overrides() const { return this->parameter_overrides_; } -NodeOptions & -NodeOptions::parameter_overrides(const std::vector & parameter_overrides) +NodeOptions & NodeOptions::parameter_overrides( + const std::vector & parameter_overrides) { this->parameter_overrides_ = parameter_overrides; return *this; } -bool -NodeOptions::use_global_arguments() const +bool NodeOptions::use_global_arguments() const { return this->node_options_->use_global_arguments; } -NodeOptions & -NodeOptions::use_global_arguments(bool use_global_arguments) +NodeOptions & NodeOptions::use_global_arguments(bool use_global_arguments) { this->node_options_.reset(); // reset node options to make it be recreated on next access. this->use_global_arguments_ = use_global_arguments; return *this; } -bool -NodeOptions::use_intra_process_comms() const +bool NodeOptions::use_intra_process_comms() const { return this->use_intra_process_comms_; } -NodeOptions & -NodeOptions::use_intra_process_comms(bool use_intra_process_comms) +NodeOptions & NodeOptions::use_intra_process_comms(bool use_intra_process_comms) { this->use_intra_process_comms_ = use_intra_process_comms; return *this; } -bool -NodeOptions::start_parameter_services() const +bool NodeOptions::start_parameter_services() const { return this->start_parameter_services_; } -NodeOptions & -NodeOptions::start_parameter_services(bool start_parameter_services) +NodeOptions & NodeOptions::start_parameter_services(bool start_parameter_services) { this->start_parameter_services_ = start_parameter_services; return *this; } -bool -NodeOptions::start_parameter_event_publisher() const +bool NodeOptions::start_parameter_event_publisher() const { return this->start_parameter_event_publisher_; } -NodeOptions & -NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publisher) +NodeOptions & NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publisher) { this->start_parameter_event_publisher_ = start_parameter_event_publisher; return *this; } -const rclcpp::QoS & -NodeOptions::parameter_event_qos() const +const rclcpp::QoS & NodeOptions::parameter_event_qos() const { return this->parameter_event_qos_; } -NodeOptions & -NodeOptions::parameter_event_qos(const rclcpp::QoS & parameter_event_qos) +NodeOptions & NodeOptions::parameter_event_qos(const rclcpp::QoS & parameter_event_qos) { this->parameter_event_qos_ = parameter_event_qos; return *this; } -const rclcpp::PublisherOptionsBase & -NodeOptions::parameter_event_publisher_options() const +const rclcpp::PublisherOptionsBase & NodeOptions::parameter_event_publisher_options() const { return parameter_event_publisher_options_; } -NodeOptions & -NodeOptions::parameter_event_publisher_options( +NodeOptions & NodeOptions::parameter_event_publisher_options( const rclcpp::PublisherOptionsBase & parameter_event_publisher_options) { parameter_event_publisher_options_ = parameter_event_publisher_options; return *this; } -bool -NodeOptions::allow_undeclared_parameters() const +bool NodeOptions::allow_undeclared_parameters() const { return this->allow_undeclared_parameters_; } -NodeOptions & -NodeOptions::allow_undeclared_parameters(bool allow_undeclared_parameters) +NodeOptions & NodeOptions::allow_undeclared_parameters(bool allow_undeclared_parameters) { this->allow_undeclared_parameters_ = allow_undeclared_parameters; return *this; } -bool -NodeOptions::automatically_declare_parameters_from_overrides() const +bool NodeOptions::automatically_declare_parameters_from_overrides() const { return this->automatically_declare_parameters_from_overrides_; } -NodeOptions & -NodeOptions::automatically_declare_parameters_from_overrides( +NodeOptions & NodeOptions::automatically_declare_parameters_from_overrides( bool automatically_declare_parameters_from_overrides) { this->automatically_declare_parameters_from_overrides_ = @@ -292,14 +267,12 @@ NodeOptions::automatically_declare_parameters_from_overrides( return *this; } -const rcl_allocator_t & -NodeOptions::allocator() const +const rcl_allocator_t & NodeOptions::allocator() const { return this->allocator_; } -NodeOptions & -NodeOptions::allocator(rcl_allocator_t allocator) +NodeOptions & NodeOptions::allocator(rcl_allocator_t allocator) { this->node_options_.reset(); // reset node options to make it be recreated on next access. this->allocator_ = allocator; @@ -308,8 +281,7 @@ NodeOptions::allocator(rcl_allocator_t allocator) // TODO(wjwwood): reuse rcutils_get_env() to avoid code duplication. // See also: https://github.com/ros2/rcl/issues/119 -size_t -NodeOptions::get_domain_id_from_env() const +size_t NodeOptions::get_domain_id_from_env() const { // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable. size_t domain_id = std::numeric_limits::max(); diff --git a/rclcpp/src/rclcpp/parameter.cpp b/rclcpp/src/rclcpp/parameter.cpp index 673e06ab9b..f7cee96666 100644 --- a/rclcpp/src/rclcpp/parameter.cpp +++ b/rclcpp/src/rclcpp/parameter.cpp @@ -22,16 +22,14 @@ #include "rclcpp/node_interfaces/node_parameters.hpp" #include "rclcpp/utilities.hpp" -using rclcpp::ParameterType; using rclcpp::Parameter; +using rclcpp::ParameterType; -Parameter::Parameter() -: name_("") +Parameter::Parameter() : name_("") { } -Parameter::Parameter(const std::string & name) -: name_(name), value_() +Parameter::Parameter(const std::string & name) : name_(name), value_() { } @@ -45,110 +43,92 @@ Parameter::Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_in { } -bool -Parameter::operator==(const Parameter & rhs) const +bool Parameter::operator==(const Parameter & rhs) const { return this->name_ == rhs.name_ && this->value_ == rhs.value_; } -bool -Parameter::operator!=(const Parameter & rhs) const +bool Parameter::operator!=(const Parameter & rhs) const { return !(*this == rhs); } -ParameterType -Parameter::get_type() const +ParameterType Parameter::get_type() const { return value_.get_type(); } -std::string -Parameter::get_type_name() const +std::string Parameter::get_type_name() const { return rclcpp::to_string(get_type()); } -const std::string & -Parameter::get_name() const +const std::string & Parameter::get_name() const { return name_; } -rcl_interfaces::msg::ParameterValue -Parameter::get_value_message() const +rcl_interfaces::msg::ParameterValue Parameter::get_value_message() const { return value_.to_value_msg(); } -const rclcpp::ParameterValue & -Parameter::get_parameter_value() const +const rclcpp::ParameterValue & Parameter::get_parameter_value() const { return value_; } -bool -Parameter::as_bool() const +bool Parameter::as_bool() const { return get_value(); } -int64_t -Parameter::as_int() const +int64_t Parameter::as_int() const { return get_value(); } -double -Parameter::as_double() const +double Parameter::as_double() const { return get_value(); } -const std::string & -Parameter::as_string() const +const std::string & Parameter::as_string() const { return get_value(); } -const std::vector & -Parameter::as_byte_array() const +const std::vector & Parameter::as_byte_array() const { return get_value(); } -const std::vector & -Parameter::as_bool_array() const +const std::vector & Parameter::as_bool_array() const { return get_value(); } -const std::vector & -Parameter::as_integer_array() const +const std::vector & Parameter::as_integer_array() const { return get_value(); } -const std::vector & -Parameter::as_double_array() const +const std::vector & Parameter::as_double_array() const { return get_value(); } -const std::vector & -Parameter::as_string_array() const +const std::vector & Parameter::as_string_array() const { return get_value(); } -Parameter -Parameter::from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter) +Parameter Parameter::from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter) { return Parameter(parameter.name, parameter.value); } -rcl_interfaces::msg::Parameter -Parameter::to_parameter_msg() const +rcl_interfaces::msg::Parameter Parameter::to_parameter_msg() const { rcl_interfaces::msg::Parameter parameter; parameter.name = name_; @@ -156,14 +136,12 @@ Parameter::to_parameter_msg() const return parameter; } -std::string -Parameter::value_to_string() const +std::string Parameter::value_to_string() const { return rclcpp::to_string(value_); } -std::string -rclcpp::_to_json_dict_entry(const Parameter & param) +std::string rclcpp::_to_json_dict_entry(const Parameter & param) { std::stringstream ss; ss << "\"" << param.get_name() << "\": "; @@ -172,22 +150,19 @@ rclcpp::_to_json_dict_entry(const Parameter & param) return ss.str(); } -std::ostream & -rclcpp::operator<<(std::ostream & os, const rclcpp::Parameter & pv) +std::ostream & rclcpp::operator<<(std::ostream & os, const rclcpp::Parameter & pv) { os << std::to_string(pv); return os; } -std::ostream & -rclcpp::operator<<(std::ostream & os, const std::vector & parameters) +std::ostream & rclcpp::operator<<(std::ostream & os, const std::vector & parameters) { os << std::to_string(parameters); return os; } -std::string -std::to_string(const rclcpp::Parameter & param) +std::string std::to_string(const rclcpp::Parameter & param) { std::stringstream ss; ss << "{\"name\": \"" << param.get_name() << "\", "; @@ -196,8 +171,7 @@ std::to_string(const rclcpp::Parameter & param) return ss.str(); } -std::string -std::to_string(const std::vector & parameters) +std::string std::to_string(const std::vector & parameters) { std::stringstream ss; ss << "{"; diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 8afbf43726..3071b25e9b 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -73,12 +73,12 @@ AsyncParametersClient::AsyncParametersClient( set_parameters_atomically_client_ = Client::make_shared( - node_base_interface.get(), - node_graph_interface, - remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically, - options); - auto set_parameters_atomically_base = std::dynamic_pointer_cast( - set_parameters_atomically_client_); + node_base_interface.get(), + node_graph_interface, + remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically, + options); + auto set_parameters_atomically_base = + std::dynamic_pointer_cast(set_parameters_atomically_client_); node_services_interface->add_client(set_parameters_atomically_base, group); list_parameters_client_ = Client::make_shared( @@ -112,7 +112,8 @@ AsyncParametersClient::AsyncParametersClient( remote_node_name, qos_profile, group) -{} +{ +} AsyncParametersClient::AsyncParametersClient( rclcpp::Node * node, @@ -127,17 +128,14 @@ AsyncParametersClient::AsyncParametersClient( remote_node_name, qos_profile, group) -{} +{ +} -std::shared_future> -AsyncParametersClient::get_parameters( +std::shared_future> AsyncParametersClient::get_parameters( const std::vector & names, - std::function< - void(std::shared_future>) - > callback) + std::function>)> callback) { - auto promise_result = - std::make_shared>>(); + auto promise_result = std::make_shared>>(); auto future_result = promise_result->get_future().share(); auto request = std::make_shared(); @@ -146,8 +144,7 @@ AsyncParametersClient::get_parameters( get_parameters_client_->async_send_request( request, [request, promise_result, future_result, callback]( - rclcpp::Client::SharedFuture cb_f) - { + rclcpp::Client::SharedFuture cb_f) { std::vector parameters; auto & pvalues = cb_f.get()->values; @@ -163,21 +160,16 @@ AsyncParametersClient::get_parameters( if (callback != nullptr) { callback(future_result); } - } - ); + }); return future_result; } -std::shared_future> -AsyncParametersClient::get_parameter_types( +std::shared_future> AsyncParametersClient::get_parameter_types( const std::vector & names, - std::function< - void(std::shared_future>) - > callback) + std::function>)> callback) { - auto promise_result = - std::make_shared>>(); + auto promise_result = std::make_shared>>(); auto future_result = promise_result->get_future().share(); auto request = std::make_shared(); @@ -186,8 +178,7 @@ AsyncParametersClient::get_parameter_types( get_parameter_types_client_->async_send_request( request, [promise_result, future_result, callback]( - rclcpp::Client::SharedFuture cb_f) - { + rclcpp::Client::SharedFuture cb_f) { std::vector types; auto & pts = cb_f.get()->types; for (auto & pt : pts) { @@ -197,8 +188,7 @@ AsyncParametersClient::get_parameter_types( if (callback != nullptr) { callback(future_result); } - } - ); + }); return future_result; } @@ -206,9 +196,8 @@ AsyncParametersClient::get_parameter_types( std::shared_future> AsyncParametersClient::set_parameters( const std::vector & parameters, - std::function< - void(std::shared_future>) - > callback) + std::function>)> + callback) { auto promise_result = std::make_shared>>(); @@ -217,21 +206,20 @@ AsyncParametersClient::set_parameters( auto request = std::make_shared(); std::transform( - parameters.begin(), parameters.end(), std::back_inserter(request->parameters), - [](rclcpp::Parameter p) {return p.to_parameter_msg();} - ); + parameters.begin(), + parameters.end(), + std::back_inserter(request->parameters), + [](rclcpp::Parameter p) { return p.to_parameter_msg(); }); set_parameters_client_->async_send_request( request, [promise_result, future_result, callback]( - rclcpp::Client::SharedFuture cb_f) - { + rclcpp::Client::SharedFuture cb_f) { promise_result->set_value(cb_f.get()->results); if (callback != nullptr) { callback(future_result); } - } - ); + }); return future_result; } @@ -239,32 +227,28 @@ AsyncParametersClient::set_parameters( std::shared_future AsyncParametersClient::set_parameters_atomically( const std::vector & parameters, - std::function< - void(std::shared_future) - > callback) + std::function)> callback) { - auto promise_result = - std::make_shared>(); + auto promise_result = std::make_shared>(); auto future_result = promise_result->get_future().share(); auto request = std::make_shared(); std::transform( - parameters.begin(), parameters.end(), std::back_inserter(request->parameters), - [](rclcpp::Parameter p) {return p.to_parameter_msg();} - ); + parameters.begin(), + parameters.end(), + std::back_inserter(request->parameters), + [](rclcpp::Parameter p) { return p.to_parameter_msg(); }); set_parameters_atomically_client_->async_send_request( request, [promise_result, future_result, callback]( - rclcpp::Client::SharedFuture cb_f) - { + rclcpp::Client::SharedFuture cb_f) { promise_result->set_value(cb_f.get()->result); if (callback != nullptr) { callback(future_result); } - } - ); + }); return future_result; } @@ -273,12 +257,9 @@ std::shared_future AsyncParametersClient::list_parameters( const std::vector & prefixes, uint64_t depth, - std::function< - void(std::shared_future) - > callback) + std::function)> callback) { - auto promise_result = - std::make_shared>(); + auto promise_result = std::make_shared>(); auto future_result = promise_result->get_future().share(); auto request = std::make_shared(); @@ -288,39 +269,32 @@ AsyncParametersClient::list_parameters( list_parameters_client_->async_send_request( request, [promise_result, future_result, callback]( - rclcpp::Client::SharedFuture cb_f) - { + rclcpp::Client::SharedFuture cb_f) { promise_result->set_value(cb_f.get()->result); if (callback != nullptr) { callback(future_result); } - } - ); + }); return future_result; } -bool -AsyncParametersClient::service_is_ready() const +bool AsyncParametersClient::service_is_ready() const { - return - get_parameters_client_->service_is_ready() && - get_parameter_types_client_->service_is_ready() && - set_parameters_client_->service_is_ready() && - list_parameters_client_->service_is_ready() && - describe_parameters_client_->service_is_ready(); + return get_parameters_client_->service_is_ready() && + get_parameter_types_client_->service_is_ready() && + set_parameters_client_->service_is_ready() && + list_parameters_client_->service_is_ready() && + describe_parameters_client_->service_is_ready(); } -bool -AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) +bool AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) { - const std::vector> clients = { - get_parameters_client_, - get_parameter_types_client_, - set_parameters_client_, - list_parameters_client_, - describe_parameters_client_ - }; + const std::vector> clients = {get_parameters_client_, + get_parameter_types_client_, + set_parameters_client_, + list_parameters_client_, + describe_parameters_client_}; for (auto & client : clients) { auto stamp = std::chrono::steady_clock::now(); if (!client->wait_for_service(timeout)) { @@ -346,7 +320,8 @@ SyncParametersClient::SyncParametersClient( node, remote_node_name, qos_profile) -{} +{ +} SyncParametersClient::SyncParametersClient( rclcpp::executor::Executor::SharedPtr executor, @@ -361,18 +336,18 @@ SyncParametersClient::SyncParametersClient( node->get_node_services_interface(), remote_node_name, qos_profile) -{} +{ +} SyncParametersClient::SyncParametersClient( - rclcpp::Node * node, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile) + rclcpp::Node * node, const std::string & remote_node_name, const rmw_qos_profile_t & qos_profile) : SyncParametersClient( std::make_shared(), node, remote_node_name, qos_profile) -{} +{ +} SyncParametersClient::SyncParametersClient( rclcpp::executor::Executor::SharedPtr executor, @@ -387,7 +362,8 @@ SyncParametersClient::SyncParametersClient( node->get_node_services_interface(), remote_node_name, qos_profile) -{} +{ +} SyncParametersClient::SyncParametersClient( rclcpp::executor::Executor::SharedPtr executor, @@ -399,8 +375,7 @@ SyncParametersClient::SyncParametersClient( const rmw_qos_profile_t & qos_profile) : executor_(executor), node_base_interface_(node_base_interface) { - async_parameters_client_ = - std::make_shared( + async_parameters_client_ = std::make_shared( node_base_interface, node_topics_interface, node_graph_interface, @@ -409,24 +384,21 @@ SyncParametersClient::SyncParametersClient( qos_profile); } -std::vector -SyncParametersClient::get_parameters(const std::vector & parameter_names) +std::vector SyncParametersClient::get_parameters( + const std::vector & parameter_names) { auto f = async_parameters_client_->get_parameters(parameter_names); using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_future_complete( - *executor_, node_base_interface_, - f) == rclcpp::executor::FutureReturnCode::SUCCESS) - { + spin_node_until_future_complete(*executor_, node_base_interface_, f) == + rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); } // Return an empty vector if unsuccessful return std::vector(); } -bool -SyncParametersClient::has_parameter(const std::string & parameter_name) +bool SyncParametersClient::has_parameter(const std::string & parameter_name) { std::vector names; names.push_back(parameter_name); @@ -434,70 +406,58 @@ SyncParametersClient::has_parameter(const std::string & parameter_name) return vars.names.size() > 0; } -std::vector -SyncParametersClient::get_parameter_types(const std::vector & parameter_names) +std::vector SyncParametersClient::get_parameter_types( + const std::vector & parameter_names) { auto f = async_parameters_client_->get_parameter_types(parameter_names); using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_future_complete( - *executor_, node_base_interface_, - f) == rclcpp::executor::FutureReturnCode::SUCCESS) - { + spin_node_until_future_complete(*executor_, node_base_interface_, f) == + rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); } return std::vector(); } -std::vector -SyncParametersClient::set_parameters( +std::vector SyncParametersClient::set_parameters( const std::vector & parameters) { auto f = async_parameters_client_->set_parameters(parameters); using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_future_complete( - *executor_, node_base_interface_, - f) == rclcpp::executor::FutureReturnCode::SUCCESS) - { + spin_node_until_future_complete(*executor_, node_base_interface_, f) == + rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); } return std::vector(); } -rcl_interfaces::msg::SetParametersResult -SyncParametersClient::set_parameters_atomically( +rcl_interfaces::msg::SetParametersResult SyncParametersClient::set_parameters_atomically( const std::vector & parameters) { auto f = async_parameters_client_->set_parameters_atomically(parameters); using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_future_complete( - *executor_, node_base_interface_, - f) == rclcpp::executor::FutureReturnCode::SUCCESS) - { + spin_node_until_future_complete(*executor_, node_base_interface_, f) == + rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); } throw std::runtime_error("Unable to get result of set parameters service call."); } -rcl_interfaces::msg::ListParametersResult -SyncParametersClient::list_parameters( - const std::vector & parameter_prefixes, - uint64_t depth) +rcl_interfaces::msg::ListParametersResult SyncParametersClient::list_parameters( + const std::vector & parameter_prefixes, uint64_t depth) { auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_future_complete( - *executor_, node_base_interface_, - f) == rclcpp::executor::FutureReturnCode::SUCCESS) - { + spin_node_until_future_complete(*executor_, node_base_interface_, f) == + rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); } diff --git a/rclcpp/src/rclcpp/parameter_events_filter.cpp b/rclcpp/src/rclcpp/parameter_events_filter.cpp index 36ef708c35..f85e4906fd 100644 --- a/rclcpp/src/rclcpp/parameter_events_filter.cpp +++ b/rclcpp/src/rclcpp/parameter_events_filter.cpp @@ -30,31 +30,27 @@ ParameterEventsFilter::ParameterEventsFilter( if (std::find(types.begin(), types.end(), EventType::NEW) != types.end()) { for (auto & new_parameter : event->new_parameters) { if (std::find(names.begin(), names.end(), new_parameter.name) != names.end()) { - result_.push_back( - EventPair(EventType::NEW, &new_parameter)); + result_.push_back(EventPair(EventType::NEW, &new_parameter)); } } } if (std::find(types.begin(), types.end(), EventType::CHANGED) != types.end()) { for (auto & changed_parameter : event->changed_parameters) { if (std::find(names.begin(), names.end(), changed_parameter.name) != names.end()) { - result_.push_back( - EventPair(EventType::CHANGED, &changed_parameter)); + result_.push_back(EventPair(EventType::CHANGED, &changed_parameter)); } } } if (std::find(types.begin(), types.end(), EventType::DELETED) != types.end()) { for (auto & deleted_parameter : event->deleted_parameters) { if (std::find(names.begin(), names.end(), deleted_parameter.name) != names.end()) { - result_.push_back( - EventPair(EventType::DELETED, &deleted_parameter)); + result_.push_back(EventPair(EventType::DELETED, &deleted_parameter)); } } } } -const std::vector & -ParameterEventsFilter::get_events() const +const std::vector & ParameterEventsFilter::get_events() const { return result_; } diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index 447e50d4b1..bb7881f371 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -17,13 +17,12 @@ #include "rclcpp/parameter_map.hpp" -using rclcpp::exceptions::InvalidParametersException; -using rclcpp::exceptions::InvalidParameterValueException; using rclcpp::ParameterMap; using rclcpp::ParameterValue; +using rclcpp::exceptions::InvalidParametersException; +using rclcpp::exceptions::InvalidParameterValueException; -ParameterMap -rclcpp::parameter_map_from(const rcl_params_t * const c_params) +ParameterMap rclcpp::parameter_map_from(const rcl_params_t * const c_params) { if (NULL == c_params) { throw InvalidParametersException("parameters struct is NULL"); @@ -68,8 +67,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) return parameters; } -ParameterValue -rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) +ParameterValue rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) { if (NULL == c_param_value) { throw InvalidParameterValueException("Passed argument is NULL"); diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index f1f9166962..5787761f66 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -32,13 +32,13 @@ ParameterService::ParameterService( const std::string node_name = node_base->get_name(); get_parameters_service_ = create_service( - node_base, node_services, + node_base, + node_services, node_name + "/" + parameter_service_names::get_parameters, [node_params]( const std::shared_ptr, const std::shared_ptr request, - std::shared_ptr response) - { + std::shared_ptr response) { for (const auto & name : request->names) { // Default construct param to NOT_SET rclcpp::Parameter param; @@ -47,44 +47,46 @@ ParameterService::ParameterService( response->values.push_back(param.get_value_message()); } }, - qos_profile, nullptr); + qos_profile, + nullptr); get_parameter_types_service_ = create_service( - node_base, node_services, + node_base, + node_services, node_name + "/" + parameter_service_names::get_parameter_types, [node_params]( const std::shared_ptr, const std::shared_ptr request, - std::shared_ptr response) - { + std::shared_ptr response) { try { auto types = node_params->get_parameter_types(request->names); std::transform( - types.cbegin(), types.cend(), - std::back_inserter(response->types), [](const uint8_t & type) { - return static_cast(type); - }); + types.cbegin(), + types.cend(), + std::back_inserter(response->types), + [](const uint8_t & type) { return static_cast(type); }); } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what()); } }, - qos_profile, nullptr); + qos_profile, + nullptr); set_parameters_service_ = create_service( - node_base, node_services, + node_base, + node_services, node_name + "/" + parameter_service_names::set_parameters, [node_params]( const std::shared_ptr, const std::shared_ptr request, - std::shared_ptr response) - { + std::shared_ptr response) { // Set parameters one-by-one, since there's no way to return a partial result if // set_parameters() fails. auto result = rcl_interfaces::msg::SetParametersResult(); for (auto & p : request->parameters) { try { - result = node_params->set_parameters_atomically( - {rclcpp::Parameter::from_parameter_msg(p)}); + result = + node_params->set_parameters_atomically({rclcpp::Parameter::from_parameter_msg(p)}); } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what()); result.successful = false; @@ -93,19 +95,21 @@ ParameterService::ParameterService( response->results.push_back(result); } }, - qos_profile, nullptr); + qos_profile, + nullptr); set_parameters_atomically_service_ = create_service( - node_base, node_services, + node_base, + node_services, node_name + "/" + parameter_service_names::set_parameters_atomically, [node_params]( const std::shared_ptr, const std::shared_ptr request, - std::shared_ptr response) - { + std::shared_ptr response) { std::vector pvariants; std::transform( - request->parameters.cbegin(), request->parameters.cend(), + request->parameters.cbegin(), + request->parameters.cend(), std::back_inserter(pvariants), [](const rcl_interfaces::msg::Parameter & p) { return rclcpp::Parameter::from_parameter_msg(p); @@ -120,16 +124,17 @@ ParameterService::ParameterService( response->result.reason = "One or more parameters wer not declared before setting"; } }, - qos_profile, nullptr); + qos_profile, + nullptr); describe_parameters_service_ = create_service( - node_base, node_services, + node_base, + node_services, node_name + "/" + parameter_service_names::describe_parameters, [node_params]( const std::shared_ptr, const std::shared_ptr request, - std::shared_ptr response) - { + std::shared_ptr response) { try { auto descriptors = node_params->describe_parameters(request->names); response->descriptors = descriptors; @@ -137,18 +142,20 @@ ParameterService::ParameterService( RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what()); } }, - qos_profile, nullptr); + qos_profile, + nullptr); list_parameters_service_ = create_service( - node_base, node_services, + node_base, + node_services, node_name + "/" + parameter_service_names::list_parameters, [node_params]( const std::shared_ptr, const std::shared_ptr request, - std::shared_ptr response) - { + std::shared_ptr response) { auto result = node_params->list_parameters(request->prefixes, request->depth); response->result = result; }, - qos_profile, nullptr); + qos_profile, + nullptr); } diff --git a/rclcpp/src/rclcpp/parameter_value.cpp b/rclcpp/src/rclcpp/parameter_value.cpp index 79c1e13e74..7848e4ed58 100644 --- a/rclcpp/src/rclcpp/parameter_value.cpp +++ b/rclcpp/src/rclcpp/parameter_value.cpp @@ -20,8 +20,7 @@ using rclcpp::ParameterType; using rclcpp::ParameterValue; -std::string -rclcpp::to_string(const ParameterType type) +std::string rclcpp::to_string(const ParameterType type) { switch (type) { case ParameterType::PARAMETER_NOT_SET: @@ -49,18 +48,15 @@ rclcpp::to_string(const ParameterType type) } } -std::ostream & -rclcpp::operator<<(std::ostream & os, const ParameterType type) +std::ostream & rclcpp::operator<<(std::ostream & os, const ParameterType type) { os << rclcpp::to_string(type); return os; } template -std::string -array_to_string( - const std::vector & array, - const std::ios::fmtflags format_flags = std::ios::dec) +std::string array_to_string( + const std::vector & array, const std::ios::fmtflags format_flags = std::ios::dec) { std::stringstream type_array; bool first_item = true; @@ -79,8 +75,7 @@ array_to_string( return type_array.str(); } -std::string -rclcpp::to_string(const ParameterValue & value) +std::string rclcpp::to_string(const ParameterValue & value) { switch (value.get_type()) { case ParameterType::PARAMETER_NOT_SET: @@ -172,7 +167,8 @@ ParameterValue::ParameterValue(const std::string & string_value) ParameterValue::ParameterValue(const char * string_value) : ParameterValue(std::string(string_value)) -{} +{ +} ParameterValue::ParameterValue(const std::vector & byte_array_value) { @@ -216,26 +212,22 @@ ParameterValue::ParameterValue(const std::vector & string_array_val value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; } -ParameterType -ParameterValue::get_type() const +ParameterType ParameterValue::get_type() const { return static_cast(value_.type); } -rcl_interfaces::msg::ParameterValue -ParameterValue::to_value_msg() const +rcl_interfaces::msg::ParameterValue ParameterValue::to_value_msg() const { return value_; } -bool -ParameterValue::operator==(const ParameterValue & rhs) const +bool ParameterValue::operator==(const ParameterValue & rhs) const { return this->value_ == rhs.value_; } -bool -ParameterValue::operator!=(const ParameterValue & rhs) const +bool ParameterValue::operator!=(const ParameterValue & rhs) const { return this->value_ != rhs.value_; } diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index a2c6ceac91..2e5e030d6c 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -44,23 +44,18 @@ PublisherBase::PublisherBase( const rosidl_message_type_support_t & type_support, const rcl_publisher_options_t & publisher_options) : rcl_node_handle_(node_base->get_shared_rcl_node_handle()), - intra_process_is_enabled_(false), intra_process_publisher_id_(0) + intra_process_is_enabled_(false), + intra_process_publisher_id_(0) { rcl_ret_t ret = rcl_publisher_init( - &publisher_handle_, - rcl_node_handle_.get(), - &type_support, - topic.c_str(), - &publisher_options); + &publisher_handle_, rcl_node_handle_.get(), &type_support, topic.c_str(), &publisher_options); if (ret != RCL_RET_OK) { if (ret == RCL_RET_TOPIC_NAME_INVALID) { auto rcl_node_handle = rcl_node_handle_.get(); // this will throw on any validation problem rcl_reset_error(); expand_topic_or_service_name( - topic, - rcl_node_get_name(rcl_node_handle), - rcl_node_get_namespace(rcl_node_handle)); + topic, rcl_node_get_name(rcl_node_handle), rcl_node_get_namespace(rcl_node_handle)); } rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher"); @@ -94,9 +89,7 @@ PublisherBase::~PublisherBase() if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl publisher handle: %s", - rcl_get_error_string().str); + "rclcpp", "Error in destruction of rcl publisher handle: %s", rcl_get_error_string().str); rcl_reset_error(); } @@ -108,21 +101,18 @@ PublisherBase::~PublisherBase() if (!ipm) { // TODO(ivanpauno): should this raise an error? RCLCPP_WARN( - rclcpp::get_logger("rclcpp"), - "Intra process manager died before than a publisher."); + rclcpp::get_logger("rclcpp"), "Intra process manager died before than a publisher."); return; } ipm->remove_publisher(intra_process_publisher_id_); } -const char * -PublisherBase::get_topic_name() const +const char * PublisherBase::get_topic_name() const { return rcl_publisher_get_topic_name(&publisher_handle_); } -size_t -PublisherBase::get_queue_size() const +size_t PublisherBase::get_queue_size() const { const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_); if (!publisher_options) { @@ -133,26 +123,22 @@ PublisherBase::get_queue_size() const return publisher_options->qos.depth; } -const rmw_gid_t & -PublisherBase::get_gid() const +const rmw_gid_t & PublisherBase::get_gid() const { return rmw_gid_; } -const rmw_gid_t & -PublisherBase::get_intra_process_gid() const +const rmw_gid_t & PublisherBase::get_intra_process_gid() const { return intra_process_rmw_gid_; } -rcl_publisher_t * -PublisherBase::get_publisher_handle() +rcl_publisher_t * PublisherBase::get_publisher_handle() { return &publisher_handle_; } -const rcl_publisher_t * -PublisherBase::get_publisher_handle() const +const rcl_publisher_t * PublisherBase::get_publisher_handle() const { return &publisher_handle_; } @@ -163,17 +149,15 @@ PublisherBase::get_event_handlers() const return event_handlers_; } -size_t -PublisherBase::get_subscription_count() const +size_t PublisherBase::get_subscription_count() const { size_t inter_process_subscription_count = 0; - rcl_ret_t status = rcl_publisher_get_subscription_count( - &publisher_handle_, - &inter_process_subscription_count); + rcl_ret_t status = + rcl_publisher_get_subscription_count(&publisher_handle_, &inter_process_subscription_count); if (RCL_RET_PUBLISHER_INVALID == status) { - rcl_reset_error(); /* next call will reset error message if not context */ + rcl_reset_error(); /* next call will reset error message if not context */ if (rcl_publisher_is_valid_except_context(&publisher_handle_)) { rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_); if (nullptr != context && !rcl_context_is_valid(context)) { @@ -188,8 +172,7 @@ PublisherBase::get_subscription_count() const return inter_process_subscription_count; } -size_t -PublisherBase::get_intra_process_subscription_count() const +size_t PublisherBase::get_intra_process_subscription_count() const { auto ipm = weak_ipm_.lock(); if (!intra_process_is_enabled_) { @@ -199,14 +182,13 @@ PublisherBase::get_intra_process_subscription_count() const // TODO(ivanpauno): should this just return silently? Or maybe return with a warning? // Same as wjwwood comment in publisher_factory create_shared_publish_callback. throw std::runtime_error( - "intra process subscriber count called after " - "destruction of intra process manager"); + "intra process subscriber count called after " + "destruction of intra process manager"); } return ipm->get_subscription_count(intra_process_publisher_id_); } -rmw_qos_profile_t -PublisherBase::get_actual_qos() const +rmw_qos_profile_t PublisherBase::get_actual_qos() const { const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(&publisher_handle_); if (!qos) { @@ -217,20 +199,17 @@ PublisherBase::get_actual_qos() const return *qos; } -bool -PublisherBase::assert_liveliness() const +bool PublisherBase::assert_liveliness() const { return RCL_RET_OK == rcl_publisher_assert_liveliness(&publisher_handle_); } -bool -PublisherBase::operator==(const rmw_gid_t & gid) const +bool PublisherBase::operator==(const rmw_gid_t & gid) const { return *this == &gid; } -bool -PublisherBase::operator==(const rmw_gid_t * gid) const +bool PublisherBase::operator==(const rmw_gid_t * gid) const { bool result = false; auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result); @@ -250,15 +229,14 @@ PublisherBase::operator==(const rmw_gid_t * gid) const return result; } -rclcpp::mapped_ring_buffer::MappedRingBufferBase::SharedPtr -PublisherBase::make_mapped_ring_buffer(size_t size) const +rclcpp::mapped_ring_buffer::MappedRingBufferBase::SharedPtr PublisherBase::make_mapped_ring_buffer( + size_t size) const { (void)size; return nullptr; } -void -PublisherBase::setup_intra_process( +void PublisherBase::setup_intra_process( uint64_t intra_process_publisher_id, IntraProcessManagerSharedPtr ipm, const rcl_publisher_options_t & intra_process_options) @@ -266,7 +244,7 @@ PublisherBase::setup_intra_process( // Intraprocess configuration is not allowed with "durability" qos policy non "volatile". if (this->get_actual_qos().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) { throw std::invalid_argument( - "intraprocess communication is not allowed with durability qos policy non-volatile"); + "intraprocess communication is not allowed with durability qos policy non-volatile"); } const char * topic_name = this->get_topic_name(); if (!topic_name) { @@ -300,15 +278,14 @@ PublisherBase::setup_intra_process( intra_process_is_enabled_ = true; // Life time of this object is tied to the publisher handle. - rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle( - &intra_process_publisher_handle_); + rmw_publisher_t * publisher_rmw_handle = + rcl_publisher_get_rmw_handle(&intra_process_publisher_handle_); if (publisher_rmw_handle == nullptr) { auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string().str; rcl_reset_error(); throw std::runtime_error(msg); } - auto rmw_ret = rmw_get_gid_for_publisher( - publisher_rmw_handle, &intra_process_rmw_gid_); + auto rmw_ret = rmw_get_gid_for_publisher(publisher_rmw_handle, &intra_process_rmw_gid_); if (rmw_ret != RMW_RET_OK) { auto msg = std::string("failed to create intra process publisher gid: ") + rmw_get_error_string().str; diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp index bec92fdff1..29d3acd759 100644 --- a/rclcpp/src/rclcpp/qos.cpp +++ b/rclcpp/src/rclcpp/qos.cpp @@ -21,10 +21,10 @@ namespace rclcpp QoSInitialization::QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg) : history_policy(history_policy_arg), depth(depth_arg) -{} +{ +} -QoSInitialization -QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos) +QoSInitialization QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos) { switch (rmw_qos.history) { case RMW_QOS_POLICY_HISTORY_KEEP_ALL: @@ -37,148 +37,127 @@ QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos) } } -KeepAll::KeepAll() -: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 0) -{} +KeepAll::KeepAll() : QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 0) +{ +} -KeepLast::KeepLast(size_t depth) -: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth) -{} +KeepLast::KeepLast(size_t depth) : QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth) +{ +} -QoS::QoS( - const QoSInitialization & qos_initialization, - const rmw_qos_profile_t & initial_profile) +QoS::QoS(const QoSInitialization & qos_initialization, const rmw_qos_profile_t & initial_profile) : rmw_qos_profile_(initial_profile) { rmw_qos_profile_.history = qos_initialization.history_policy; rmw_qos_profile_.depth = qos_initialization.depth; } -QoS::QoS(size_t history_depth) -: QoS(KeepLast(history_depth)) -{} +QoS::QoS(size_t history_depth) : QoS(KeepLast(history_depth)) +{ +} -rmw_qos_profile_t & -QoS::get_rmw_qos_profile() +rmw_qos_profile_t & QoS::get_rmw_qos_profile() { return rmw_qos_profile_; } -const rmw_qos_profile_t & -QoS::get_rmw_qos_profile() const +const rmw_qos_profile_t & QoS::get_rmw_qos_profile() const { return rmw_qos_profile_; } -QoS & -QoS::history(rmw_qos_history_policy_t history) +QoS & QoS::history(rmw_qos_history_policy_t history) { rmw_qos_profile_.history = history; return *this; } -QoS & -QoS::keep_last(size_t depth) +QoS & QoS::keep_last(size_t depth) { rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; rmw_qos_profile_.depth = depth; return *this; } -QoS & -QoS::keep_all() +QoS & QoS::keep_all() { rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; rmw_qos_profile_.depth = 0; return *this; } -QoS & -QoS::reliability(rmw_qos_reliability_policy_t reliability) +QoS & QoS::reliability(rmw_qos_reliability_policy_t reliability) { rmw_qos_profile_.reliability = reliability; return *this; } -QoS & -QoS::reliable() +QoS & QoS::reliable() { return this->reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); } -QoS & -QoS::best_effort() +QoS & QoS::best_effort() { return this->reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); } -QoS & -QoS::durability(rmw_qos_durability_policy_t durability) +QoS & QoS::durability(rmw_qos_durability_policy_t durability) { rmw_qos_profile_.durability = durability; return *this; } -QoS & -QoS::durability_volatile() +QoS & QoS::durability_volatile() { return this->durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); } -QoS & -QoS::transient_local() +QoS & QoS::transient_local() { return this->durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); } -QoS & -QoS::deadline(rmw_time_t deadline) +QoS & QoS::deadline(rmw_time_t deadline) { rmw_qos_profile_.deadline = deadline; return *this; } -QoS & -QoS::deadline(const rclcpp::Duration & deadline) +QoS & QoS::deadline(const rclcpp::Duration & deadline) { return this->deadline(deadline.to_rmw_time()); } -QoS & -QoS::lifespan(rmw_time_t lifespan) +QoS & QoS::lifespan(rmw_time_t lifespan) { rmw_qos_profile_.lifespan = lifespan; return *this; } -QoS & -QoS::lifespan(const rclcpp::Duration & lifespan) +QoS & QoS::lifespan(const rclcpp::Duration & lifespan) { return this->lifespan(lifespan.to_rmw_time()); } -QoS & -QoS::liveliness(rmw_qos_liveliness_policy_t liveliness) +QoS & QoS::liveliness(rmw_qos_liveliness_policy_t liveliness) { rmw_qos_profile_.liveliness = liveliness; return *this; } -QoS & -QoS::liveliness_lease_duration(rmw_time_t liveliness_lease_duration) +QoS & QoS::liveliness_lease_duration(rmw_time_t liveliness_lease_duration) { rmw_qos_profile_.liveliness_lease_duration = liveliness_lease_duration; return *this; } -QoS & -QoS::liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration) +QoS & QoS::liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration) { return this->liveliness_lease_duration(liveliness_lease_duration.to_rmw_time()); } -QoS & -QoS::avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions) +QoS & QoS::avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions) { rmw_qos_profile_.avoid_ros_namespace_conventions = avoid_ros_namespace_conventions; return *this; @@ -186,22 +165,27 @@ QoS::avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions) SensorDataQoS::SensorDataQoS(const QoSInitialization & qos_initialization) : QoS(qos_initialization, rmw_qos_profile_sensor_data) -{} +{ +} ParametersQoS::ParametersQoS(const QoSInitialization & qos_initialization) : QoS(qos_initialization, rmw_qos_profile_parameters) -{} +{ +} ServicesQoS::ServicesQoS(const QoSInitialization & qos_initialization) : QoS(qos_initialization, rmw_qos_profile_services_default) -{} +{ +} ParameterEventsQoS::ParameterEventsQoS(const QoSInitialization & qos_initialization) : QoS(qos_initialization, rmw_qos_profile_parameter_events) -{} +{ +} SystemDefaultsQoS::SystemDefaultsQoS(const QoSInitialization & qos_initialization) : QoS(qos_initialization, rmw_qos_profile_system_default) -{} +{ +} } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 75072f799a..6dfa5c0fb2 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -21,22 +21,19 @@ QOSEventHandlerBase::~QOSEventHandlerBase() { if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); + "rclcpp", "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); rcl_reset_error(); } } /// Get the number of ready events. -size_t -QOSEventHandlerBase::get_number_of_ready_events() +size_t QOSEventHandlerBase::get_number_of_ready_events() { return 1; } /// Add the Waitable to a wait set. -bool -QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +bool QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_); if (RCL_RET_OK != ret) { @@ -46,8 +43,7 @@ QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) } /// Check if the Waitable is ready. -bool -QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) +bool QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) { return wait_set->events[wait_set_event_index_] == &event_handle_; } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index c7f8ff9449..fb57a8292d 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -27,39 +27,35 @@ using rclcpp::ServiceBase; -ServiceBase::ServiceBase(std::shared_ptr node_handle) -: node_handle_(node_handle) -{} +ServiceBase::ServiceBase(std::shared_ptr node_handle) : node_handle_(node_handle) +{ +} ServiceBase::~ServiceBase() -{} +{ +} -const char * -ServiceBase::get_service_name() +const char * ServiceBase::get_service_name() { return rcl_service_get_service_name(this->get_service_handle().get()); } -std::shared_ptr -ServiceBase::get_service_handle() +std::shared_ptr ServiceBase::get_service_handle() { return service_handle_; } -std::shared_ptr -ServiceBase::get_service_handle() const +std::shared_ptr ServiceBase::get_service_handle() const { return service_handle_; } -rcl_node_t * -ServiceBase::get_rcl_node_handle() +rcl_node_t * ServiceBase::get_rcl_node_handle() { return node_handle_.get(); } -const rcl_node_t * -ServiceBase::get_rcl_node_handle() const +const rcl_node_t * ServiceBase::get_rcl_node_handle() const { return node_handle_.get(); } diff --git a/rclcpp/src/rclcpp/signal_handler.cpp b/rclcpp/src/rclcpp/signal_handler.cpp index dbdc1ae157..ab7a7bee40 100644 --- a/rclcpp/src/rclcpp/signal_handler.cpp +++ b/rclcpp/src/rclcpp/signal_handler.cpp @@ -51,14 +51,12 @@ sem_t SignalHandler::signal_handler_sem_; // it, because the destructor of SignalHandler uses this logger object. static rclcpp::Logger g_logger = rclcpp::get_logger("rclcpp"); -rclcpp::Logger & -SignalHandler::get_logger() +rclcpp::Logger & SignalHandler::get_logger() { return g_logger; } -SignalHandler & -SignalHandler::get_global_signal_handler() +SignalHandler & SignalHandler::get_global_signal_handler() { // This is initialized after the g_logger static global, ensuring // SignalHandler::get_logger() may be called from the destructor of @@ -80,8 +78,7 @@ SignalHandler::get_global_signal_handler() return signal_handler; } -bool -SignalHandler::install() +bool SignalHandler::install() { std::lock_guard lock(install_mutex_); bool already_installed = installed_.exchange(true); @@ -113,8 +110,7 @@ SignalHandler::install() return true; } -bool -SignalHandler::uninstall() +bool SignalHandler::uninstall() { std::lock_guard lock(install_mutex_); bool installed = installed_.exchange(false); @@ -137,8 +133,7 @@ SignalHandler::uninstall() return true; } -bool -SignalHandler::is_installed() +bool SignalHandler::is_installed() { return installed_.load(); } @@ -151,7 +146,8 @@ SignalHandler::~SignalHandler() RCLCPP_ERROR( get_logger(), "caught %s exception when uninstalling the sigint handler in rclcpp::~SignalHandler: %s", - rmw::impl::cpp::demangle(exc).c_str(), exc.what()); + rmw::impl::cpp::demangle(exc).c_str(), + exc.what()); } catch (...) { RCLCPP_ERROR( get_logger(), @@ -160,8 +156,7 @@ SignalHandler::~SignalHandler() } RCLCPP_LOCAL -void -__safe_strerror(int errnum, char * buffer, size_t buffer_length) +void __safe_strerror(int errnum, char * buffer, size_t buffer_length) { #if defined(_WIN32) strerror_s(buffer, buffer_length, errnum); @@ -181,10 +176,8 @@ __safe_strerror(int errnum, char * buffer, size_t buffer_length) #endif } -SignalHandler::signal_handler_type -SignalHandler::set_signal_handler( - int signal_value, - const SignalHandler::signal_handler_type & signal_handler) +SignalHandler::signal_handler_type SignalHandler::set_signal_handler( + int signal_value, const SignalHandler::signal_handler_type & signal_handler) { bool signal_handler_install_failed; SignalHandler::signal_handler_type old_signal_handler; @@ -206,19 +199,16 @@ SignalHandler::set_signal_handler( return old_signal_handler; } -void -SignalHandler::signal_handler_common() +void SignalHandler::signal_handler_common() { signal_received_.store(true); RCLCPP_DEBUG( - get_logger(), - "signal_handler(): SIGINT received, notifying deferred signal handler"); + get_logger(), "signal_handler(): SIGINT received, notifying deferred signal handler"); notify_signal_handler(); } #if defined(RCLCPP_HAS_SIGACTION) -void -SignalHandler::signal_handler(int signal_value, siginfo_t * siginfo, void * context) +void SignalHandler::signal_handler(int signal_value, siginfo_t * siginfo, void * context) { RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value); @@ -228,9 +218,9 @@ SignalHandler::signal_handler(int signal_value, siginfo_t * siginfo, void * cont } } else { if ( - old_signal_handler_.sa_handler != NULL && // Is set + old_signal_handler_.sa_handler != NULL && // Is set old_signal_handler_.sa_handler != SIG_DFL && // Is not default - old_signal_handler_.sa_handler != SIG_IGN) // Is not ignored + old_signal_handler_.sa_handler != SIG_IGN) // Is not ignored { old_signal_handler_.sa_handler(signal_value); } @@ -239,8 +229,7 @@ SignalHandler::signal_handler(int signal_value, siginfo_t * siginfo, void * cont signal_handler_common(); } #else -void -SignalHandler::signal_handler(int signal_value) +void SignalHandler::signal_handler(int signal_value) { RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value); @@ -252,8 +241,7 @@ SignalHandler::signal_handler(int signal_value) } #endif -void -SignalHandler::deferred_signal_handler() +void SignalHandler::deferred_signal_handler() { while (true) { if (signal_received_.exchange(false)) { @@ -279,14 +267,13 @@ SignalHandler::deferred_signal_handler() } } -void -SignalHandler::setup_wait_for_signal() +void SignalHandler::setup_wait_for_signal() { #if defined(_WIN32) signal_handler_sem_ = CreateSemaphore( - NULL, // default security attributes - 0, // initial semaphore count - 1, // maximum semaphore count + NULL, // default security attributes + 0, // initial semaphore count + 1, // maximum semaphore count NULL); // unnamed semaphore if (NULL == signal_handler_sem_) { throw std::runtime_error("CreateSemaphore() failed in setup_wait_for_signal()"); @@ -301,8 +288,7 @@ SignalHandler::setup_wait_for_signal() wait_for_signal_is_setup_.store(true); } -void -SignalHandler::teardown_wait_for_signal() noexcept +void SignalHandler::teardown_wait_for_signal() noexcept { if (!wait_for_signal_is_setup_.exchange(false)) { return; @@ -318,8 +304,7 @@ SignalHandler::teardown_wait_for_signal() noexcept #endif } -void -SignalHandler::wait_for_signal() +void SignalHandler::wait_for_signal() { if (!wait_for_signal_is_setup_.load()) { RCLCPP_ERROR(get_logger(), "called wait_for_signal() before setup_wait_for_signal()"); @@ -330,7 +315,8 @@ SignalHandler::wait_for_signal() switch (dw_wait_result) { case WAIT_ABANDONED: RCLCPP_ERROR( - get_logger(), "WaitForSingleObject() failed in wait_for_signal() with WAIT_ABANDONED: %s", + get_logger(), + "WaitForSingleObject() failed in wait_for_signal() with WAIT_ABANDONED: %s", GetLastError()); break; case WAIT_OBJECT_0: @@ -345,7 +331,8 @@ SignalHandler::wait_for_signal() break; default: RCLCPP_ERROR( - get_logger(), "WaitForSingleObject() gave unknown return in wait_for_signal(): %s", + get_logger(), + "WaitForSingleObject() gave unknown return in wait_for_signal(): %s", GetLastError()); } #elif defined(__APPLE__) @@ -358,8 +345,7 @@ SignalHandler::wait_for_signal() #endif } -void -SignalHandler::notify_signal_handler() noexcept +void SignalHandler::notify_signal_handler() noexcept { if (!wait_for_signal_is_setup_.load()) { return; diff --git a/rclcpp/src/rclcpp/signal_handler.hpp b/rclcpp/src/rclcpp/signal_handler.hpp index 81cb6e180c..c14a2b8c79 100644 --- a/rclcpp/src/rclcpp/signal_handler.hpp +++ b/rclcpp/src/rclcpp/signal_handler.hpp @@ -55,33 +55,26 @@ class SignalHandler final { public: /// Return the global singleton of this class. - static - SignalHandler & - get_global_signal_handler(); + static SignalHandler & get_global_signal_handler(); /// Return a global singleton logger to avoid needing to create it everywhere. - static - rclcpp::Logger & - get_logger(); + static rclcpp::Logger & get_logger(); /// Install the signal handler for SIGINT and start the dedicated signal handling thread. /** * Also stores the current signal handler to be called on SIGINT and to * restore when uninstalling this signal handler. */ - bool - install(); + bool install(); /// Uninstall the signal handler for SIGINT and join the dedicated singal handling thread. /** * Also restores the previous signal handler. */ - bool - uninstall(); + bool uninstall(); /// Return true if installed, false otherwise. - bool - is_installed(); + bool is_installed(); private: SignalHandler() = default; @@ -97,39 +90,29 @@ class SignalHandler final static SignalHandler::signal_handler_type old_signal_handler_; /// Set the signal handler function. - static - SignalHandler::signal_handler_type - set_signal_handler(int signal_value, const SignalHandler::signal_handler_type & signal_handler); + static SignalHandler::signal_handler_type set_signal_handler( + int signal_value, const SignalHandler::signal_handler_type & signal_handler); /// Common signal handler code between sigaction and non-sigaction versions. - static - void - signal_handler_common(); + static void signal_handler_common(); #if defined(RCLCPP_HAS_SIGACTION) /// Signal handler function. - static - void - signal_handler(int signal_value, siginfo_t * siginfo, void * context); + static void signal_handler(int signal_value, siginfo_t * siginfo, void * context); #else /// Signal handler function. - static - void - signal_handler(int signal_value); + static void signal_handler(int signal_value); #endif /// Target of the dedicated signal handling thread. - void - deferred_signal_handler(); + void deferred_signal_handler(); /// Setup anything that is necessary for wait_for_signal() or notify_signal_handler(). /** * This must be called before wait_for_signal() or notify_signal_handler(). * This is not thread-safe. */ - static - void - setup_wait_for_signal(); + static void setup_wait_for_signal(); /// Undo all setup done in setup_wait_for_signal(). /** @@ -137,9 +120,7 @@ class SignalHandler final * * This is not thread-safe. */ - static - void - teardown_wait_for_signal() noexcept; + static void teardown_wait_for_signal() noexcept; /// Wait for a notification from notify_signal_handler() in a signal safe way. /** @@ -147,9 +128,7 @@ class SignalHandler final * * This is not thread-safe. */ - static - void - wait_for_signal(); + static void wait_for_signal(); /// Notify blocking wait_for_signal() calls in a signal safe way. /** @@ -158,9 +137,7 @@ class SignalHandler final * * This is thread-safe. */ - static - void - notify_signal_handler() noexcept; + static void notify_signal_handler() noexcept; // Whether or not a signal has been received. static std::atomic_bool signal_received_; diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 51891505b4..76781fbac9 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -41,24 +41,23 @@ SubscriptionBase::SubscriptionBase( type_support_(type_support_handle), is_serialized_(is_serialized) { - auto custom_deletor = [node_handle](rcl_subscription_t * rcl_subs) - { - if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) { - RCLCPP_ERROR( - rclcpp::get_node_logger(node_handle.get()).get_child("rclcpp"), - "Error in destruction of rcl subscription handle: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } - delete rcl_subs; - }; - - subscription_handle_ = std::shared_ptr( - new rcl_subscription_t, custom_deletor); + auto custom_deletor = [node_handle](rcl_subscription_t * rcl_subs) { + if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle.get()).get_child("rclcpp"), + "Error in destruction of rcl subscription handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete rcl_subs; + }; + + subscription_handle_ = + std::shared_ptr(new rcl_subscription_t, custom_deletor); *subscription_handle_.get() = rcl_get_zero_initialized_subscription(); - intra_process_subscription_handle_ = std::shared_ptr( - new rcl_subscription_t, custom_deletor); + intra_process_subscription_handle_ = + std::shared_ptr(new rcl_subscription_t, custom_deletor); *intra_process_subscription_handle_.get() = rcl_get_zero_initialized_subscription(); rcl_ret_t ret = rcl_subscription_init( @@ -73,9 +72,7 @@ SubscriptionBase::SubscriptionBase( // this will throw on any validation problem rcl_reset_error(); expand_topic_or_service_name( - topic_name, - rcl_node_get_name(rcl_node_handle), - rcl_node_get_namespace(rcl_node_handle)); + topic_name, rcl_node_get_name(rcl_node_handle), rcl_node_get_namespace(rcl_node_handle)); } rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription"); @@ -91,33 +88,29 @@ SubscriptionBase::~SubscriptionBase() if (!ipm) { // TODO(ivanpauno): should this raise an error? RCLCPP_WARN( - rclcpp::get_logger("rclcpp"), - "Intra process manager died before than a subscription."); + rclcpp::get_logger("rclcpp"), "Intra process manager died before than a subscription."); return; } ipm->remove_subscription(intra_process_subscription_id_); } -const char * -SubscriptionBase::get_topic_name() const +const char * SubscriptionBase::get_topic_name() const { return rcl_subscription_get_topic_name(subscription_handle_.get()); } -std::shared_ptr -SubscriptionBase::get_subscription_handle() +std::shared_ptr SubscriptionBase::get_subscription_handle() { return subscription_handle_; } -const std::shared_ptr -SubscriptionBase::get_subscription_handle() const +const std::shared_ptr SubscriptionBase::get_subscription_handle() const { return subscription_handle_; } -const std::shared_ptr -SubscriptionBase::get_intra_process_subscription_handle() const +const std::shared_ptr SubscriptionBase::get_intra_process_subscription_handle() + const { return intra_process_subscription_handle_; } @@ -128,8 +121,7 @@ SubscriptionBase::get_event_handlers() const return event_handlers_; } -rmw_qos_profile_t -SubscriptionBase::get_actual_qos() const +rmw_qos_profile_t SubscriptionBase::get_actual_qos() const { const rmw_qos_profile_t * qos = rcl_subscription_get_actual_qos(subscription_handle_.get()); if (!qos) { @@ -140,26 +132,22 @@ SubscriptionBase::get_actual_qos() const return *qos; } -const rosidl_message_type_support_t & -SubscriptionBase::get_message_type_support_handle() const +const rosidl_message_type_support_t & SubscriptionBase::get_message_type_support_handle() const { return type_support_; } -bool -SubscriptionBase::is_serialized() const +bool SubscriptionBase::is_serialized() const { return is_serialized_; } -size_t -SubscriptionBase::get_publisher_count() const +size_t SubscriptionBase::get_publisher_count() const { size_t inter_process_publisher_count = 0; rmw_ret_t status = rcl_subscription_get_publisher_count( - subscription_handle_.get(), - &inter_process_publisher_count); + subscription_handle_.get(), &inter_process_publisher_count); if (RCL_RET_OK != status) { rclcpp::exceptions::throw_from_rcl_error(status, "failed to get get publisher count"); diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index b7f0bd2d4a..b74c9e73bf 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -32,8 +32,7 @@ namespace { -rcl_time_point_t -init_time_point(rcl_clock_type_t & clock_type) +rcl_time_point_t init_time_point(rcl_clock_type_t & clock_type) { rcl_time_point_t time_point; time_point.clock_type = clock_type; @@ -63,15 +62,12 @@ Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type) rcl_time_.nanoseconds = nanoseconds; } -Time::Time(const Time & rhs) -: rcl_time_(rhs.rcl_time_) +Time::Time(const Time & rhs) : rcl_time_(rhs.rcl_time_) { rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds; } -Time::Time( - const builtin_interfaces::msg::Time & time_msg, - rcl_clock_type_t ros_time) +Time::Time(const builtin_interfaces::msg::Time & time_msg, rcl_clock_type_t ros_time) { rcl_time_ = init_time_point(ros_time); if (time_msg.sec < 0) { @@ -82,8 +78,7 @@ Time::Time( rcl_time_.nanoseconds += time_msg.nanosec; } -Time::Time(const rcl_time_point_t & time_point) -: rcl_time_(time_point) +Time::Time(const rcl_time_point_t & time_point) : rcl_time_(time_point) { // noop } @@ -100,21 +95,18 @@ Time::operator builtin_interfaces::msg::Time() const return msg_time; } -Time & -Time::operator=(const Time & rhs) +Time & Time::operator=(const Time & rhs) { rcl_time_ = rhs.rcl_time_; return *this; } -Time & -Time::operator=(const builtin_interfaces::msg::Time & time_msg) +Time & Time::operator=(const builtin_interfaces::msg::Time & time_msg) { if (time_msg.sec < 0) { throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); } - rcl_clock_type_t ros_time = RCL_ROS_TIME; rcl_time_ = init_time_point(ros_time); // TODO(tfoote) hard coded ROS here @@ -123,8 +115,7 @@ Time::operator=(const builtin_interfaces::msg::Time & time_msg) return *this; } -bool -Time::operator==(const rclcpp::Time & rhs) const +bool Time::operator==(const rclcpp::Time & rhs) const { if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { throw std::runtime_error("can't compare times with different time sources"); @@ -133,14 +124,12 @@ Time::operator==(const rclcpp::Time & rhs) const return rcl_time_.nanoseconds == rhs.rcl_time_.nanoseconds; } -bool -Time::operator!=(const rclcpp::Time & rhs) const +bool Time::operator!=(const rclcpp::Time & rhs) const { return !(*this == rhs); } -bool -Time::operator<(const rclcpp::Time & rhs) const +bool Time::operator<(const rclcpp::Time & rhs) const { if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { throw std::runtime_error("can't compare times with different time sources"); @@ -149,8 +138,7 @@ Time::operator<(const rclcpp::Time & rhs) const return rcl_time_.nanoseconds < rhs.rcl_time_.nanoseconds; } -bool -Time::operator<=(const rclcpp::Time & rhs) const +bool Time::operator<=(const rclcpp::Time & rhs) const { if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { throw std::runtime_error("can't compare times with different time sources"); @@ -159,8 +147,7 @@ Time::operator<=(const rclcpp::Time & rhs) const return rcl_time_.nanoseconds <= rhs.rcl_time_.nanoseconds; } -bool -Time::operator>=(const rclcpp::Time & rhs) const +bool Time::operator>=(const rclcpp::Time & rhs) const { if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { throw std::runtime_error("can't compare times with different time sources"); @@ -169,8 +156,7 @@ Time::operator>=(const rclcpp::Time & rhs) const return rcl_time_.nanoseconds >= rhs.rcl_time_.nanoseconds; } -bool -Time::operator>(const rclcpp::Time & rhs) const +bool Time::operator>(const rclcpp::Time & rhs) const { if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { throw std::runtime_error("can't compare times with different time sources"); @@ -179,8 +165,7 @@ Time::operator>(const rclcpp::Time & rhs) const return rcl_time_.nanoseconds > rhs.rcl_time_.nanoseconds; } -Time -Time::operator+(const rclcpp::Duration & rhs) const +Time Time::operator+(const rclcpp::Duration & rhs) const { if (rclcpp::add_will_overflow(rhs.nanoseconds(), this->nanoseconds())) { throw std::overflow_error("addition leads to int64_t overflow"); @@ -191,14 +176,13 @@ Time::operator+(const rclcpp::Duration & rhs) const return Time(this->nanoseconds() + rhs.nanoseconds(), this->get_clock_type()); } -Duration -Time::operator-(const rclcpp::Time & rhs) const +Duration Time::operator-(const rclcpp::Time & rhs) const { if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { throw std::runtime_error( - std::string("can't subtract times with different time sources [") + - std::to_string(rcl_time_.clock_type) + " != " + - std::to_string(rhs.rcl_time_.clock_type) + "]"); + std::string("can't subtract times with different time sources [") + + std::to_string(rcl_time_.clock_type) + " != " + std::to_string(rhs.rcl_time_.clock_type) + + "]"); } if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.rcl_time_.nanoseconds)) { @@ -212,8 +196,7 @@ Time::operator-(const rclcpp::Time & rhs) const return Duration(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds); } -Time -Time::operator-(const rclcpp::Duration & rhs) const +Time Time::operator-(const rclcpp::Duration & rhs) const { if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.nanoseconds())) { throw std::overflow_error("time subtraction leads to int64_t overflow"); @@ -225,26 +208,22 @@ Time::operator-(const rclcpp::Duration & rhs) const return Time(rcl_time_.nanoseconds - rhs.nanoseconds(), rcl_time_.clock_type); } -int64_t -Time::nanoseconds() const +int64_t Time::nanoseconds() const { return rcl_time_.nanoseconds; } -double -Time::seconds() const +double Time::seconds() const { return std::chrono::duration(std::chrono::nanoseconds(rcl_time_.nanoseconds)).count(); } -rcl_clock_type_t -Time::get_clock_type() const +rcl_clock_type_t Time::get_clock_type() const { return rcl_time_.clock_type; } -Time -operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs) +Time operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs) { if (rclcpp::add_will_overflow(rhs.nanoseconds(), lhs.nanoseconds())) { throw std::overflow_error("addition leads to int64_t overflow"); @@ -255,11 +234,9 @@ operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs) return Time(lhs.nanoseconds() + rhs.nanoseconds(), rhs.get_clock_type()); } -Time -Time::max() +Time Time::max() { return Time(std::numeric_limits::max(), 999999999); } - } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index bde1c50d93..5c96c80a99 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -34,16 +34,12 @@ namespace rclcpp { TimeSource::TimeSource(std::shared_ptr node) -: logger_(rclcpp::get_logger("rclcpp")), - clock_subscription_(nullptr), - ros_time_active_(false) +: logger_(rclcpp::get_logger("rclcpp")), clock_subscription_(nullptr), ros_time_active_(false) { this->attachNode(node); } -TimeSource::TimeSource() -: logger_(rclcpp::get_logger("rclcpp")), - ros_time_active_(false) +TimeSource::TimeSource() : logger_(rclcpp::get_logger("rclcpp")), ros_time_active_(false) { } @@ -85,9 +81,7 @@ void TimeSource::attachNode( const char * use_sim_time_name = "use_sim_time"; if (!node_parameters_->has_parameter(use_sim_time_name)) { use_sim_time_param = node_parameters_->declare_parameter( - use_sim_time_name, - rclcpp::ParameterValue(false), - rcl_interfaces::msg::ParameterDescriptor()); + use_sim_time_name, rclcpp::ParameterValue(false), rcl_interfaces::msg::ParameterDescriptor()); } else { use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value(); } @@ -101,14 +95,14 @@ void TimeSource::attachNode( // TODO(wjwwood): use set_on_parameters_set_callback to catch the type mismatch, // before the use_sim_time parameter can ever be set to an invalid value RCLCPP_ERROR( - logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'", + logger_, + "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'", rclcpp::to_string(use_sim_time_param.get_type()).c_str()); } // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( - node_topics_, - std::bind(&TimeSource::on_parameter_event, this, std::placeholders::_1)); + node_topics_, std::bind(&TimeSource::on_parameter_event, this, std::placeholders::_1)); } void TimeSource::detachNode() @@ -156,15 +150,15 @@ void TimeSource::detachClock(std::shared_ptr clock) TimeSource::~TimeSource() { if ( - node_base_ || node_topics_ || node_graph_ || node_services_ || - node_logging_ || node_clock_ || node_parameters_) - { + node_base_ || node_topics_ || node_graph_ || node_services_ || node_logging_ || node_clock_ || + node_parameters_) { this->detachNode(); } } void TimeSource::set_clock( - const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled, + const builtin_interfaces::msg::Time::SharedPtr msg, + bool set_ros_time_enabled, std::shared_ptr clock) { // Do change @@ -176,8 +170,7 @@ void TimeSource::set_clock( auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), rclcpp::Time(*msg).nanoseconds()); if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to set ros_time_override_status"); + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to set ros_time_override_status"); } } @@ -210,8 +203,7 @@ void TimeSource::create_clock_sub() node_topics_, "/clock", rclcpp::QoS(QoSInitialization::from_rmw(rmw_qos_profile_default)), - std::bind(&TimeSource::clock_cb, this, std::placeholders::_1) - ); + std::bind(&TimeSource::clock_cb, this, std::placeholders::_1)); } void TimeSource::destroy_clock_sub() @@ -227,9 +219,11 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S return; } // Filter for only 'use_sim_time' being added or changed. - rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"}, + rclcpp::ParameterEventsFilter filter( + event, + {"use_sim_time"}, {rclcpp::ParameterEventsFilter::EventType::NEW, - rclcpp::ParameterEventsFilter::EventType::CHANGED}); + rclcpp::ParameterEventsFilter::EventType::CHANGED}); for (auto & it : filter.get_events()) { if (it.second->value.type != ParameterType::PARAMETER_BOOL) { RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool"); @@ -246,10 +240,10 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S } } // Handle the case that use_sim_time was deleted. - rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"}, - {rclcpp::ParameterEventsFilter::EventType::DELETED}); + rclcpp::ParameterEventsFilter deleted( + event, {"use_sim_time"}, {rclcpp::ParameterEventsFilter::EventType::DELETED}); for (auto & it : deleted.get_events()) { - (void) it; // if there is a match it's already matched, don't bother reading it. + (void)it; // if there is a match it's already matched, don't bother reading it. // If the parameter is deleted mark it as unset but dont' change state. parameter_state_ = UNSET; } @@ -259,8 +253,7 @@ void TimeSource::enable_ros_time(std::shared_ptr clock) { auto ret = rcl_enable_ros_time_override(&clock->rcl_clock_); if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to enable ros_time_override_status"); + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to enable ros_time_override_status"); } } @@ -268,8 +261,7 @@ void TimeSource::disable_ros_time(std::shared_ptr clock) { auto ret = rcl_disable_ros_time_override(&clock->rcl_clock_); if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to enable ros_time_override_status"); + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to enable ros_time_override_status"); } } diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 8291a12f5d..d3b1176e58 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -15,8 +15,8 @@ #include "rclcpp/timer.hpp" #include -#include #include +#include #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/exceptions.hpp" @@ -37,49 +37,47 @@ TimerBase::TimerBase( auto rcl_context = context->get_rcl_context(); - timer_handle_ = std::shared_ptr( - new rcl_timer_t, [ = ](rcl_timer_t * timer) mutable - { - if (rcl_timer_fini(timer) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - delete timer; - // Captured shared pointers by copy, reset to make sure timer is finalized before clock - clock.reset(); - rcl_context.reset(); - }); + timer_handle_ = std::shared_ptr(new rcl_timer_t, [=](rcl_timer_t * timer) mutable { + if (rcl_timer_fini(timer) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + delete timer; + // Captured shared pointers by copy, reset to make sure timer is finalized before clock + clock.reset(); + rcl_context.reset(); + }); *timer_handle_.get() = rcl_get_zero_initialized_timer(); rcl_clock_t * clock_handle = clock_->get_clock_handle(); if ( rcl_timer_init( - timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr, - rcl_get_default_allocator()) != RCL_RET_OK) - { + timer_handle_.get(), + clock_handle, + rcl_context.get(), + period.count(), + nullptr, + rcl_get_default_allocator()) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str); + "rclcpp", "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str); rcl_reset_error(); } } TimerBase::~TimerBase() -{} +{ +} -void -TimerBase::cancel() +void TimerBase::cancel() { if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) { throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string().str); } } -bool -TimerBase::is_canceled() +bool TimerBase::is_canceled() { bool is_canceled = false; rcl_ret_t ret = rcl_timer_is_canceled(timer_handle_.get(), &is_canceled); @@ -89,16 +87,14 @@ TimerBase::is_canceled() return is_canceled; } -void -TimerBase::reset() +void TimerBase::reset() { if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) { throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string().str); } } -bool -TimerBase::is_ready() +bool TimerBase::is_ready() { bool ready = false; if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) { @@ -107,24 +103,18 @@ TimerBase::is_ready() return ready; } -std::chrono::nanoseconds -TimerBase::time_until_trigger() +std::chrono::nanoseconds TimerBase::time_until_trigger() { int64_t time_until_next_call = 0; if ( - rcl_timer_get_time_until_next_call( - timer_handle_.get(), - &time_until_next_call) != RCL_RET_OK) - { + rcl_timer_get_time_until_next_call(timer_handle_.get(), &time_until_next_call) != RCL_RET_OK) { throw std::runtime_error( - std::string( - "Timer could not get time until next call: ") + rcl_get_error_string().str); + std::string("Timer could not get time until next call: ") + rcl_get_error_string().str); } return std::chrono::nanoseconds(time_until_next_call); } -std::shared_ptr -TimerBase::get_timer_handle() +std::shared_ptr TimerBase::get_timer_handle() { return timer_handle_; } diff --git a/rclcpp/src/rclcpp/type_support.cpp b/rclcpp/src/rclcpp/type_support.cpp index 27dfccb98b..55e24e9445 100644 --- a/rclcpp/src/rclcpp/type_support.cpp +++ b/rclcpp/src/rclcpp/type_support.cpp @@ -31,86 +31,71 @@ const rosidl_message_type_support_t * rclcpp::type_support::get_intra_process_message_msg_type_support() { return rosidl_typesupport_cpp::get_message_type_support_handle< - rcl_interfaces::msg::IntraProcessMessage - >(); + rcl_interfaces::msg::IntraProcessMessage>(); } -const rosidl_message_type_support_t * -rclcpp::type_support::get_parameter_event_msg_type_support() +const rosidl_message_type_support_t * rclcpp::type_support::get_parameter_event_msg_type_support() { return rosidl_typesupport_cpp::get_message_type_support_handle< - rcl_interfaces::msg::ParameterEvent - >(); + rcl_interfaces::msg::ParameterEvent>(); } const rosidl_message_type_support_t * rclcpp::type_support::get_set_parameters_result_msg_type_support() { return rosidl_typesupport_cpp::get_message_type_support_handle< - rcl_interfaces::msg::SetParametersResult - >(); + rcl_interfaces::msg::SetParametersResult>(); } const rosidl_message_type_support_t * rclcpp::type_support::get_parameter_descriptor_msg_type_support() { return rosidl_typesupport_cpp::get_message_type_support_handle< - rcl_interfaces::msg::ParameterDescriptor - >(); + rcl_interfaces::msg::ParameterDescriptor>(); } const rosidl_message_type_support_t * rclcpp::type_support::get_list_parameters_result_msg_type_support() { return rosidl_typesupport_cpp::get_message_type_support_handle< - rcl_interfaces::msg::ListParametersResult - >(); + rcl_interfaces::msg::ListParametersResult>(); } -const rosidl_service_type_support_t * -rclcpp::type_support::get_get_parameters_srv_type_support() +const rosidl_service_type_support_t * rclcpp::type_support::get_get_parameters_srv_type_support() { return rosidl_typesupport_cpp::get_service_type_support_handle< - rcl_interfaces::srv::GetParameters - >(); + rcl_interfaces::srv::GetParameters>(); } const rosidl_service_type_support_t * rclcpp::type_support::get_get_parameter_types_srv_type_support() { return rosidl_typesupport_cpp::get_service_type_support_handle< - rcl_interfaces::srv::GetParameterTypes - >(); + rcl_interfaces::srv::GetParameterTypes>(); } -const rosidl_service_type_support_t * -rclcpp::type_support::get_set_parameters_srv_type_support() +const rosidl_service_type_support_t * rclcpp::type_support::get_set_parameters_srv_type_support() { return rosidl_typesupport_cpp::get_service_type_support_handle< - rcl_interfaces::srv::SetParameters - >(); + rcl_interfaces::srv::SetParameters>(); } -const rosidl_service_type_support_t * -rclcpp::type_support::get_list_parameters_srv_type_support() +const rosidl_service_type_support_t * rclcpp::type_support::get_list_parameters_srv_type_support() { return rosidl_typesupport_cpp::get_service_type_support_handle< - rcl_interfaces::srv::ListParameters - >(); + rcl_interfaces::srv::ListParameters>(); } const rosidl_service_type_support_t * rclcpp::type_support::get_describe_parameters_srv_type_support() { return rosidl_typesupport_cpp::get_service_type_support_handle< - rcl_interfaces::srv::DescribeParameters - >(); + rcl_interfaces::srv::DescribeParameters>(); } const rosidl_service_type_support_t * rclcpp::type_support::get_set_parameters_atomically_srv_type_support() { return rosidl_typesupport_cpp::get_service_type_support_handle< - rcl_interfaces::srv::SetParametersAtomically - >(); + rcl_interfaces::srv::SetParametersAtomically>(); } diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 83008b4e53..2adccfe1e8 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -27,8 +27,7 @@ namespace rclcpp { -void -init(int argc, char const * const argv[], const InitOptions & init_options) +void init(int argc, char const * const argv[], const InitOptions & init_options) { using contexts::default_context::get_global_default_context; get_global_default_context()->init(argc, argv, init_options); @@ -36,36 +35,29 @@ init(int argc, char const * const argv[], const InitOptions & init_options) install_signal_handlers(); } -bool -install_signal_handlers() +bool install_signal_handlers() { return SignalHandler::get_global_signal_handler().install(); } -bool -signal_handlers_installed() +bool signal_handlers_installed() { return SignalHandler::get_global_signal_handler().is_installed(); } -bool -uninstall_signal_handlers() +bool uninstall_signal_handlers() { return SignalHandler::get_global_signal_handler().uninstall(); } -std::vector -init_and_remove_ros_arguments( - int argc, - char const * const argv[], - const InitOptions & init_options) +std::vector init_and_remove_ros_arguments( + int argc, char const * const argv[], const InitOptions & init_options) { init(argc, argv, init_options); return remove_ros_arguments(argc, argv); } -std::vector -remove_ros_arguments(int argc, char const * const argv[]) +std::vector remove_ros_arguments(int argc, char const * const argv[]) { rcl_allocator_t alloc = rcl_get_default_allocator(); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); @@ -80,12 +72,7 @@ remove_ros_arguments(int argc, char const * const argv[]) int nonros_argc = 0; const char ** nonros_argv = NULL; - ret = rcl_remove_ros_arguments( - argv, - &parsed_args, - alloc, - &nonros_argc, - &nonros_argv); + ret = rcl_remove_ros_arguments(argv, &parsed_args, alloc, &nonros_argc, &nonros_argv); if (RCL_RET_OK != ret || nonros_argc < 0) { // Not using throw_from_rcl_error, because we may need to append deallocation failures. @@ -95,8 +82,8 @@ remove_ros_arguments(int argc, char const * const argv[]) alloc.deallocate(nonros_argv, alloc.state); } if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) { - base_exc.formatted_message += std::string( - ", failed also to cleanup parsed arguments, leaking memory: ") + + base_exc.formatted_message += + std::string(", failed also to cleanup parsed arguments, leaking memory: ") + rcl_get_error_string().str; rcl_reset_error(); } @@ -115,15 +102,13 @@ remove_ros_arguments(int argc, char const * const argv[]) ret = rcl_arguments_fini(&parsed_args); if (RCL_RET_OK != ret) { - exceptions::throw_from_rcl_error( - ret, "failed to cleanup parsed arguments, leaking memory"); + exceptions::throw_from_rcl_error(ret, "failed to cleanup parsed arguments, leaking memory"); } return return_arguments; } -bool -ok(Context::SharedPtr context) +bool ok(Context::SharedPtr context) { using contexts::default_context::get_global_default_context; if (nullptr == context) { @@ -132,14 +117,12 @@ ok(Context::SharedPtr context) return context->is_valid(); } -bool -is_initialized(Context::SharedPtr context) +bool is_initialized(Context::SharedPtr context) { return ok(context); } -bool -shutdown(Context::SharedPtr context, const std::string & reason) +bool shutdown(Context::SharedPtr context, const std::string & reason) { using contexts::default_context::get_global_default_context; auto default_context = get_global_default_context(); @@ -153,8 +136,7 @@ shutdown(Context::SharedPtr context, const std::string & reason) return ret; } -void -on_shutdown(std::function callback, Context::SharedPtr context) +void on_shutdown(std::function callback, Context::SharedPtr context) { using contexts::default_context::get_global_default_context; if (nullptr == context) { @@ -163,8 +145,7 @@ on_shutdown(std::function callback, Context::SharedPtr context) context->on_shutdown(callback); } -bool -sleep_for(const std::chrono::nanoseconds & nanoseconds, Context::SharedPtr context) +bool sleep_for(const std::chrono::nanoseconds & nanoseconds, Context::SharedPtr context) { using contexts::default_context::get_global_default_context; if (nullptr == context) { @@ -173,14 +154,12 @@ sleep_for(const std::chrono::nanoseconds & nanoseconds, Context::SharedPtr conte return context->sleep_for(nanoseconds); } -const char * -get_c_string(const char * string_in) +const char * get_c_string(const char * string_in) { return string_in; } -const char * -get_c_string(const std::string & string_in) +const char * get_c_string(const std::string & string_in) { return string_in.c_str(); } diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 542b10a016..d2e46244c2 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -16,38 +16,32 @@ using rclcpp::Waitable; -size_t -Waitable::get_number_of_ready_subscriptions() +size_t Waitable::get_number_of_ready_subscriptions() { return 0u; } -size_t -Waitable::get_number_of_ready_timers() +size_t Waitable::get_number_of_ready_timers() { return 0u; } -size_t -Waitable::get_number_of_ready_clients() +size_t Waitable::get_number_of_ready_clients() { return 0u; } -size_t -Waitable::get_number_of_ready_events() +size_t Waitable::get_number_of_ready_events() { return 0u; } -size_t -Waitable::get_number_of_ready_services() +size_t Waitable::get_number_of_ready_services() { return 0u; } -size_t -Waitable::get_number_of_ready_guard_conditions() +size_t Waitable::get_number_of_ready_guard_conditions() { return 0u; } diff --git a/rclcpp/test/executors/test_multi_threaded_executor.cpp b/rclcpp/test/executors/test_multi_threaded_executor.cpp index e6c2bcb7f5..29755ab42c 100644 --- a/rclcpp/test/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/executors/test_multi_threaded_executor.cpp @@ -15,13 +15,13 @@ #include #include -#include #include +#include #include "rclcpp/exceptions.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/node.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp/executors.hpp" #include "rcl_interfaces/msg/intra_process_message.hpp" @@ -41,7 +41,8 @@ class TestMultiThreadedExecutor : public ::testing::Test /* Test that timers are not taken multiple times when using reentrant callback groups. */ -TEST_F(TestMultiThreadedExecutor, timer_over_take) { +TEST_F(TestMultiThreadedExecutor, timer_over_take) +{ #ifdef __linux__ // This seems to be the most effective way to force the bug to happen on Linux. // This is unnecessary on MacOS, since the default scheduler causes it. @@ -68,34 +69,34 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { std::mutex last_mutex; auto last = system_clock.now(); - std::atomic_int timer_count {0}; + std::atomic_int timer_count{0}; auto timer_callback = [&timer_count, &executor, &system_clock, &last_mutex, &last]() { - // While this tolerance is a little wide, if the bug occurs, the next step will - // happen almost instantly. The purpose of this test is not to measure the jitter - // in timers, just assert that a reasonable amount of time has passed. - const double PERIOD = 0.1f; - const double TOLERANCE = 0.025f; + // While this tolerance is a little wide, if the bug occurs, the next step will + // happen almost instantly. The purpose of this test is not to measure the jitter + // in timers, just assert that a reasonable amount of time has passed. + const double PERIOD = 0.1f; + const double TOLERANCE = 0.025f; - rclcpp::Time now = system_clock.now(); - timer_count++; + rclcpp::Time now = system_clock.now(); + timer_count++; - if (timer_count > 5) { - executor.cancel(); - } + if (timer_count > 5) { + executor.cancel(); + } - { - std::lock_guard lock(last_mutex); - double diff = std::abs((now - last).nanoseconds()) / 1.0e9; - last = now; + { + std::lock_guard lock(last_mutex); + double diff = std::abs((now - last).nanoseconds()) / 1.0e9; + last = now; - if (diff < PERIOD - TOLERANCE || diff > PERIOD + TOLERANCE) { - executor.cancel(); - ASSERT_GT(diff, PERIOD - TOLERANCE); - ASSERT_LT(diff, PERIOD + TOLERANCE); - } + if (diff < PERIOD - TOLERANCE || diff > PERIOD + TOLERANCE) { + executor.cancel(); + ASSERT_GT(diff, PERIOD - TOLERANCE); + ASSERT_LT(diff, PERIOD + TOLERANCE); } - }; + } + }; auto timer = node->create_wall_timer(100ms, timer_callback, cbg); executor.add_node(node); diff --git a/rclcpp/test/node_interfaces/node_wrapper.hpp b/rclcpp/test/node_interfaces/node_wrapper.hpp index f4319f545d..e62659d8de 100644 --- a/rclcpp/test/node_interfaces/node_wrapper.hpp +++ b/rclcpp/test/node_interfaces/node_wrapper.hpp @@ -23,39 +23,59 @@ class NodeWrapper { public: - explicit NodeWrapper(const std::string & name) - : node(std::make_shared(name)) - {} - - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_base_interface() {return this->node->get_node_base_interface();} - - rclcpp::node_interfaces::NodeClockInterface::SharedPtr - get_node_clock_interface() {return this->node->get_node_clock_interface();} - - rclcpp::node_interfaces::NodeGraphInterface::SharedPtr - get_node_graph_interface() {return this->node->get_node_graph_interface();} - - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr - get_node_logging_interface() {return this->node->get_node_logging_interface();} - - rclcpp::node_interfaces::NodeTimersInterface::SharedPtr - get_node_timers_interface() {return this->node->get_node_timers_interface();} - - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr - get_node_topics_interface() {return this->node->get_node_topics_interface();} - - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr - get_node_services_interface() {return this->node->get_node_services_interface();} - - rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr - get_node_waitables_interface() {return this->node->get_node_waitables_interface();} - - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr - get_node_parameters_interface() {return this->node->get_node_parameters_interface();} - - rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr - get_node_time_source_interface() {return this->node->get_node_time_source_interface();} + explicit NodeWrapper(const std::string & name) : node(std::make_shared(name)) + { + } + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() + { + return this->node->get_node_base_interface(); + } + + rclcpp::node_interfaces::NodeClockInterface::SharedPtr get_node_clock_interface() + { + return this->node->get_node_clock_interface(); + } + + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr get_node_graph_interface() + { + return this->node->get_node_graph_interface(); + } + + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr get_node_logging_interface() + { + return this->node->get_node_logging_interface(); + } + + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr get_node_timers_interface() + { + return this->node->get_node_timers_interface(); + } + + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr get_node_topics_interface() + { + return this->node->get_node_topics_interface(); + } + + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr get_node_services_interface() + { + return this->node->get_node_services_interface(); + } + + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr get_node_waitables_interface() + { + return this->node->get_node_waitables_interface(); + } + + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface() + { + return this->node->get_node_parameters_interface(); + } + + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface() + { + return this->node->get_node_time_source_interface(); + } private: rclcpp::Node::SharedPtr node; diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp index 121f64126f..157f22c182 100644 --- a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp +++ b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp @@ -14,8 +14,8 @@ #include -#include "rclcpp/rclcpp.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/rclcpp.hpp" int main(void) { diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp index 4f5b8311af..d2aa18687b 100644 --- a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp +++ b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp @@ -14,8 +14,8 @@ #include -#include "rclcpp/rclcpp.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/rclcpp.hpp" #include "../node_wrapper.hpp" diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp index ed147418f5..efbd7888af 100644 --- a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp +++ b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp @@ -14,8 +14,8 @@ #include -#include "rclcpp/rclcpp.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/rclcpp.hpp" int main(void) { diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp index 6d7c19cac7..13944806fa 100644 --- a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp +++ b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp @@ -14,8 +14,8 @@ #include -#include "rclcpp/rclcpp.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/rclcpp.hpp" #include "../node_wrapper.hpp" diff --git a/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp b/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp index 015cade1e4..97af11db70 100644 --- a/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp +++ b/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp @@ -47,71 +47,64 @@ class TestGetNodeInterfaces : public ::testing::Test rclcpp::Node::SharedPtr TestGetNodeInterfaces::node = nullptr; std::shared_ptr TestGetNodeInterfaces::wrapped_node = nullptr; -TEST_F(TestGetNodeInterfaces, rclcpp_node_shared_ptr) { +TEST_F(TestGetNodeInterfaces, rclcpp_node_shared_ptr) +{ auto result = rclcpp::node_interfaces::get_node_topics_interface(this->node); static_assert( - std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, - decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + std::is_same::value, + "expected rclcpp::node_interfaces::NodeTopicsInterface *"); } -TEST_F(TestGetNodeInterfaces, node_shared_ptr) { +TEST_F(TestGetNodeInterfaces, node_shared_ptr) +{ auto result = rclcpp::node_interfaces::get_node_topics_interface(this->wrapped_node); static_assert( - std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, - decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + std::is_same::value, + "expected rclcpp::node_interfaces::NodeTopicsInterface *"); } -TEST_F(TestGetNodeInterfaces, rclcpp_node_reference) { +TEST_F(TestGetNodeInterfaces, rclcpp_node_reference) +{ rclcpp::Node & node_reference = *this->node; auto result = rclcpp::node_interfaces::get_node_topics_interface(node_reference); static_assert( - std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, - decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + std::is_same::value, + "expected rclcpp::node_interfaces::NodeTopicsInterface *"); } -TEST_F(TestGetNodeInterfaces, node_reference) { +TEST_F(TestGetNodeInterfaces, node_reference) +{ NodeWrapper & wrapped_node_reference = *this->wrapped_node; auto result = rclcpp::node_interfaces::get_node_topics_interface(wrapped_node_reference); static_assert( - std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, - decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + std::is_same::value, + "expected rclcpp::node_interfaces::NodeTopicsInterface *"); } -TEST_F(TestGetNodeInterfaces, rclcpp_node_pointer) { +TEST_F(TestGetNodeInterfaces, rclcpp_node_pointer) +{ rclcpp::Node * node_pointer = this->node.get(); auto result = rclcpp::node_interfaces::get_node_topics_interface(node_pointer); static_assert( - std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, - decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + std::is_same::value, + "expected rclcpp::node_interfaces::NodeTopicsInterface *"); } -TEST_F(TestGetNodeInterfaces, node_pointer) { +TEST_F(TestGetNodeInterfaces, node_pointer) +{ NodeWrapper * wrapped_node_pointer = this->wrapped_node.get(); auto result = rclcpp::node_interfaces::get_node_topics_interface(wrapped_node_pointer); static_assert( - std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, - decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + std::is_same::value, + "expected rclcpp::node_interfaces::NodeTopicsInterface *"); } -TEST_F(TestGetNodeInterfaces, interface_shared_pointer) { +TEST_F(TestGetNodeInterfaces, interface_shared_pointer) +{ std::shared_ptr interface_shared_ptr = this->node->get_node_topics_interface(); auto result = rclcpp::node_interfaces::get_node_topics_interface(interface_shared_ptr); static_assert( - std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, - decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + std::is_same::value, + "expected rclcpp::node_interfaces::NodeTopicsInterface *"); } diff --git a/rclcpp/test/test_client.cpp b/rclcpp/test/test_client.cpp index fb3fefdb47..3a5e4e428c 100644 --- a/rclcpp/test/test_client.cpp +++ b/rclcpp/test/test_client.cpp @@ -14,8 +14,8 @@ #include -#include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" @@ -68,7 +68,8 @@ class TestClientSub : public ::testing::Test /* Testing client construction and destruction. */ -TEST_F(TestClient, construction_and_destruction) { +TEST_F(TestClient, construction_and_destruction) +{ using rcl_interfaces::srv::ListParameters; { auto client = node->create_client("service"); @@ -76,13 +77,13 @@ TEST_F(TestClient, construction_and_destruction) { { ASSERT_THROW( - { - auto client = node->create_client("invalid_service?"); - }, rclcpp::exceptions::InvalidServiceNameError); + { auto client = node->create_client("invalid_service?"); }, + rclcpp::exceptions::InvalidServiceNameError); } } -TEST_F(TestClient, construction_with_free_function) { +TEST_F(TestClient, construction_with_free_function) +{ { auto client = rclcpp::create_client( node->get_node_base_interface(), @@ -94,22 +95,24 @@ TEST_F(TestClient, construction_with_free_function) { } { ASSERT_THROW( - { - auto client = rclcpp::create_client( - node->get_node_base_interface(), - node->get_node_graph_interface(), - node->get_node_services_interface(), - "invalid_?service", - rmw_qos_profile_services_default, - nullptr); - }, rclcpp::exceptions::InvalidServiceNameError); + { + auto client = rclcpp::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "invalid_?service", + rmw_qos_profile_services_default, + nullptr); + }, + rclcpp::exceptions::InvalidServiceNameError); } } /* Testing client construction and destruction for subnodes. */ -TEST_F(TestClientSub, construction_and_destruction) { +TEST_F(TestClientSub, construction_and_destruction) +{ using rcl_interfaces::srv::ListParameters; { auto client = subnode->create_client("service"); @@ -118,8 +121,7 @@ TEST_F(TestClientSub, construction_and_destruction) { { ASSERT_THROW( - { - auto client = node->create_client("invalid_service?"); - }, rclcpp::exceptions::InvalidServiceNameError); + { auto client = node->create_client("invalid_service?"); }, + rclcpp::exceptions::InvalidServiceNameError); } } diff --git a/rclcpp/test/test_create_timer.cpp b/rclcpp/test/test_create_timer.cpp index cff870a8a4..f2b05209f4 100644 --- a/rclcpp/test/test_create_timer.cpp +++ b/rclcpp/test/test_create_timer.cpp @@ -18,10 +18,10 @@ #include #include +#include "node_interfaces/node_wrapper.hpp" #include "rclcpp/create_timer.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/node.hpp" -#include "node_interfaces/node_wrapper.hpp" using namespace std::chrono_literals; @@ -33,11 +33,8 @@ TEST(TestCreateTimer, timer_executes) std::atomic got_callback{false}; rclcpp::TimerBase::SharedPtr timer; - timer = rclcpp::create_timer( - node, - node->get_clock(), - rclcpp::Duration(0ms), - [&got_callback, &timer]() { + timer = + rclcpp::create_timer(node, node->get_clock(), rclcpp::Duration(0ms), [&got_callback, &timer]() { got_callback = true; timer->cancel(); }); @@ -55,9 +52,6 @@ TEST(TestCreateTimer, call_with_node_wrapper_compiles) rclcpp::TimerBase::SharedPtr timer; timer = rclcpp::create_timer( - node, - node.get_node_clock_interface()->get_clock(), - rclcpp::Duration(0ms), - []() {}); + node, node.get_node_clock_interface()->get_clock(), rclcpp::Duration(0ms), []() {}); rclcpp::shutdown(); } diff --git a/rclcpp/test/test_duration.cpp b/rclcpp/test/test_duration.cpp index b77f0467d4..5213a01335 100644 --- a/rclcpp/test/test_duration.cpp +++ b/rclcpp/test/test_duration.cpp @@ -22,9 +22,8 @@ #include "rcl/error_handling.h" #include "rcl/time.h" #include "rclcpp/clock.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp/duration.hpp" - +#include "rclcpp/rclcpp.hpp" using namespace std::chrono_literals; @@ -36,7 +35,8 @@ class TestDuration : public ::testing::Test // TODO(tfoote) Implement conversion methods // } -TEST(TestDuration, operators) { +TEST(TestDuration, operators) +{ rclcpp::Duration old(1, 0); rclcpp::Duration young(2, 0); @@ -67,7 +67,8 @@ TEST(TestDuration, operators) { EXPECT_TRUE(time == assignment_op_duration); } -TEST(TestDuration, chrono_overloads) { +TEST(TestDuration, chrono_overloads) +{ int64_t ns = 123456789l; auto chrono_ns = std::chrono::nanoseconds(ns); auto d1 = rclcpp::Duration(ns); @@ -86,7 +87,8 @@ TEST(TestDuration, chrono_overloads) { EXPECT_EQ(chrono_float_seconds, d5.to_chrono()); } -TEST(TestDuration, overflows) { +TEST(TestDuration, overflows) +{ rclcpp::Duration max(std::numeric_limits::max()); rclcpp::Duration min(std::numeric_limits::min()); @@ -107,7 +109,8 @@ TEST(TestDuration, overflows) { EXPECT_THROW(base_d_neg * 4, std::underflow_error); } -TEST(TestDuration, negative_duration) { +TEST(TestDuration, negative_duration) +{ rclcpp::Duration assignable_duration = rclcpp::Duration(0) - rclcpp::Duration(5, 0); { @@ -130,7 +133,8 @@ TEST(TestDuration, negative_duration) { } } -TEST(TestDuration, maximum_duration) { +TEST(TestDuration, maximum_duration) +{ rclcpp::Duration max_duration = rclcpp::Duration::max(); rclcpp::Duration max(std::numeric_limits::max(), 999999999); @@ -140,14 +144,16 @@ TEST(TestDuration, maximum_duration) { static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000; static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS; -TEST(TestDuration, from_seconds) { +TEST(TestDuration, from_seconds) +{ EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0.0)); EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0)); EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(1.5)); EXPECT_EQ(rclcpp::Duration(-ONE_AND_HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(-1.5)); } -TEST(TestDuration, std_chrono_constructors) { +TEST(TestDuration, std_chrono_constructors) +{ EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0.0s)); EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0s)); EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration(1.5s)); diff --git a/rclcpp/test/test_executor.cpp b/rclcpp/test/test_executor.cpp index 95371415fc..2a35a468d5 100644 --- a/rclcpp/test/test_executor.cpp +++ b/rclcpp/test/test_executor.cpp @@ -50,7 +50,8 @@ class TestExecutors : public ::testing::Test }; // Make sure that executors detach from nodes when destructing -TEST_F(TestExecutors, detachOnDestruction) { +TEST_F(TestExecutors, detachOnDestruction) +{ { rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); @@ -62,7 +63,8 @@ TEST_F(TestExecutors, detachOnDestruction) { } // Make sure that the executor can automatically remove expired nodes correctly -TEST_F(TestExecutors, addTemporaryNode) { +TEST_F(TestExecutors, addTemporaryNode) +{ rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(std::make_shared("temporary_node")); EXPECT_NO_THROW(executor.spin_some()); diff --git a/rclcpp/test/test_expand_topic_or_service_name.cpp b/rclcpp/test/test_expand_topic_or_service_name.cpp index 98c10609b4..23ed6403ba 100644 --- a/rclcpp/test/test_expand_topic_or_service_name.cpp +++ b/rclcpp/test/test_expand_topic_or_service_name.cpp @@ -20,7 +20,8 @@ /* Testing expand_topic_or_service_name. */ -TEST(TestExpandTopicOrServiceName, normal) { +TEST(TestExpandTopicOrServiceName, normal) +{ using rclcpp::expand_topic_or_service_name; { ASSERT_EQ("/ns/chatter", expand_topic_or_service_name("chatter", "node", "/ns")); @@ -30,51 +31,52 @@ TEST(TestExpandTopicOrServiceName, normal) { /* Testing exceptions of expand_topic_or_service_name. */ -TEST(TestExpandTopicOrServiceName, exceptions) { +TEST(TestExpandTopicOrServiceName, exceptions) +{ using rclcpp::expand_topic_or_service_name; { ASSERT_THROW( - { - expand_topic_or_service_name("chatter", "invalid_node?", "/ns"); - }, rclcpp::exceptions::InvalidNodeNameError); + { expand_topic_or_service_name("chatter", "invalid_node?", "/ns"); }, + rclcpp::exceptions::InvalidNodeNameError); } { ASSERT_THROW( - { - expand_topic_or_service_name("chatter", "node", "/invalid_ns?"); - }, rclcpp::exceptions::InvalidNamespaceError); + { expand_topic_or_service_name("chatter", "node", "/invalid_ns?"); }, + rclcpp::exceptions::InvalidNamespaceError); } { ASSERT_THROW( - { - expand_topic_or_service_name("chatter/42invalid", "node", "/ns"); - }, rclcpp::exceptions::InvalidTopicNameError); + { expand_topic_or_service_name("chatter/42invalid", "node", "/ns"); }, + rclcpp::exceptions::InvalidTopicNameError); } { ASSERT_THROW( - { - // this one will only fail on the "full" topic name validation check - expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns"); - }, rclcpp::exceptions::InvalidTopicNameError); + { + // this one will only fail on the "full" topic name validation check + expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns"); + }, + rclcpp::exceptions::InvalidTopicNameError); } { ASSERT_THROW( - { - // is_service = true - expand_topic_or_service_name("chatter/42invalid", "node", "/ns", true); - }, rclcpp::exceptions::InvalidServiceNameError); + { + // is_service = true + expand_topic_or_service_name("chatter/42invalid", "node", "/ns", true); + }, + rclcpp::exceptions::InvalidServiceNameError); } { ASSERT_THROW( - { - // is_service = true - // this one will only fail on the "full" topic name validation check - expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns", true); - }, rclcpp::exceptions::InvalidServiceNameError); + { + // is_service = true + // this one will only fail on the "full" topic name validation check + expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns", true); + }, + rclcpp::exceptions::InvalidServiceNameError); } } diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp index 563f7f6ec3..c4aae17d12 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/rclcpp/test/test_externally_defined_services.cpp @@ -14,18 +14,18 @@ #include -#include #include +#include -#include "rclcpp/node.hpp" #include "rclcpp/any_service_callback.hpp" -#include "rclcpp/service.hpp" +#include "rclcpp/node.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/service.hpp" #include "rcl/service.h" -#include "test_msgs/srv/empty.hpp" #include "test_msgs/srv/empty.h" +#include "test_msgs/srv/empty.hpp" class TestExternallyDefinedServices : public ::testing::Test { @@ -36,13 +36,14 @@ class TestExternallyDefinedServices : public ::testing::Test } }; -void -callback( - const std::shared_ptr/*req*/, - std::shared_ptr/*resp*/) -{} +void callback( + const std::shared_ptr /*req*/, + std::shared_ptr /*resp*/) +{ +} -TEST_F(TestExternallyDefinedServices, default_behavior) { +TEST_F(TestExternallyDefinedServices, default_behavior) +{ auto node_handle = rclcpp::Node::make_shared("base_node"); try { @@ -54,7 +55,8 @@ TEST_F(TestExternallyDefinedServices, default_behavior) { SUCCEED(); } -TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) { +TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) +{ auto node_handle = rclcpp::Node::make_shared("base_node"); // mock for externally defined service @@ -66,8 +68,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) { // expect fail try { rclcpp::Service( - node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), - &service_handle, cb); + node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), &service_handle, cb); } catch (const std::runtime_error &) { SUCCEED(); return; @@ -76,7 +77,8 @@ TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) { FAIL(); } -TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { +TEST_F(TestExternallyDefinedServices, extern_defined_initialized) +{ auto node_handle = rclcpp::Node::make_shared("base_node"); // mock for externally defined service @@ -87,7 +89,9 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { rcl_ret_t ret = rcl_service_init( &service_handle, node_handle->get_node_base_interface()->get_rcl_node_handle(), - ts, "base_node_service", &service_options); + ts, + "base_node_service", + &service_options); if (ret != RCL_RET_OK) { FAIL(); return; @@ -97,8 +101,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { try { rclcpp::Service( - node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), - &service_handle, cb); + node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), &service_handle, cb); } catch (const std::runtime_error &) { FAIL(); return; @@ -106,8 +109,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { // Destruct the service ret = rcl_service_fini( - &service_handle, - node_handle->get_node_base_interface()->get_rcl_node_handle()); + &service_handle, node_handle->get_node_base_interface()->get_rcl_node_handle()); if (ret != RCL_RET_OK) { FAIL(); return; @@ -116,7 +118,8 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { SUCCEED(); } -TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { +TEST_F(TestExternallyDefinedServices, extern_defined_destructor) +{ auto node_handle = rclcpp::Node::make_shared("base_node"); // mock for externally defined service @@ -127,7 +130,9 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { rcl_ret_t ret = rcl_service_init( &service_handle, node_handle->get_node_base_interface()->get_rcl_node_handle(), - ts, "base_node_service", &service_options); + ts, + "base_node_service", + &service_options); if (ret != RCL_RET_OK) { FAIL(); return; @@ -137,8 +142,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { { // Call constructor rclcpp::Service srv_cpp( - node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), - &service_handle, cb); + node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), &service_handle, cb); // Call destructor } @@ -149,8 +153,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { // Destruct the service ret = rcl_service_fini( - &service_handle, - node_handle->get_node_base_interface()->get_rcl_node_handle()); + &service_handle, node_handle->get_node_base_interface()->get_rcl_node_handle()); if (ret != RCL_RET_OK) { FAIL(); return; diff --git a/rclcpp/test/test_find_weak_nodes.cpp b/rclcpp/test/test_find_weak_nodes.cpp index f4e58e8d19..eb4bfe227a 100644 --- a/rclcpp/test/test_find_weak_nodes.cpp +++ b/rclcpp/test/test_find_weak_nodes.cpp @@ -15,10 +15,10 @@ #include #include -#include "rclcpp/strategies/allocator_memory_strategy.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/strategies/allocator_memory_strategy.hpp" class TestFindWeakNodes : public ::testing::Test { @@ -29,7 +29,8 @@ class TestFindWeakNodes : public ::testing::Test } }; -TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) { +TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) +{ // GIVEN // A vector of weak pointers to nodes auto memory_strategy = std::make_shared< @@ -57,7 +58,8 @@ TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) { memory_strategy->clear_handles(); } -TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) { +TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) +{ // GIVEN // A vector of weak pointers to nodes, all valid auto memory_strategy = std::make_shared< diff --git a/rclcpp/test/test_function_traits.cpp b/rclcpp/test/test_function_traits.cpp index a9e6e8d261..5adac864ae 100644 --- a/rclcpp/test/test_function_traits.cpp +++ b/rclcpp/test/test_function_traits.cpp @@ -105,8 +105,7 @@ template< typename FunctorT, std::size_t Arity = 0, typename std::enable_if< - rclcpp::function_traits::arity_comparator::value>::type * = nullptr -> + rclcpp::function_traits::arity_comparator::value>::type * = nullptr> int func_accept_callback(FunctorT callback) { return callback(); @@ -114,10 +113,8 @@ int func_accept_callback(FunctorT callback) template< typename FunctorT, - typename std::enable_if< - rclcpp::function_traits::check_arguments::value - >::type * = nullptr -> + typename std::enable_if::value>::type * = + nullptr> int func_accept_callback(FunctorT callback) { int a = 4; @@ -127,9 +124,7 @@ int func_accept_callback(FunctorT callback) template< typename FunctorT, typename std::enable_if< - rclcpp::function_traits::check_arguments::value - >::type * = nullptr -> + rclcpp::function_traits::check_arguments::value>::type * = nullptr> int func_accept_callback(FunctorT callback) { int a = 5; @@ -140,9 +135,7 @@ int func_accept_callback(FunctorT callback) template< typename FunctorT, typename std::enable_if< - rclcpp::function_traits::check_arguments::value - >::type * = nullptr -> + rclcpp::function_traits::check_arguments::value>::type * = nullptr> int func_accept_callback(FunctorT callback) { int a = 7; @@ -154,15 +147,10 @@ template< typename FunctorT, std::size_t Arity = 0, typename std::enable_if< - rclcpp::function_traits::arity_comparator::value - >::type * = nullptr, - typename std::enable_if< - std::is_same< - typename rclcpp::function_traits::function_traits::return_type, - double - >::value - >::type * = nullptr -> + rclcpp::function_traits::arity_comparator::value>::type * = nullptr, + typename std::enable_if::return_type, + double>::value>::type * = nullptr> double func_accept_callback_return_type(FunctorT callback) { return callback(); @@ -172,15 +160,10 @@ template< typename FunctorT, std::size_t Arity = 0, typename std::enable_if< - rclcpp::function_traits::arity_comparator::value - >::type * = nullptr, - typename std::enable_if< - std::is_same< - typename rclcpp::function_traits::function_traits::return_type, - std::string - >::value - >::type * = nullptr -> + rclcpp::function_traits::arity_comparator::value>::type * = nullptr, + typename std::enable_if::return_type, + std::string>::value>::type * = nullptr> std::string func_accept_callback_return_type(FunctorT callback) { return callback(); @@ -189,7 +172,8 @@ std::string func_accept_callback_return_type(FunctorT callback) /* Tests that funcion_traits calculates arity of several functors. */ -TEST(TestFunctionTraits, arity) { +TEST(TestFunctionTraits, arity) +{ // Test regular functions static_assert( rclcpp::function_traits::function_traits::arity == 0, @@ -208,26 +192,24 @@ TEST(TestFunctionTraits, arity) { "Functor only accepts two arguments"); // Test lambdas - auto lambda_no_args = []() { - return 0; - }; + auto lambda_no_args = []() { return 0; }; auto lambda_one_int = [](int one) { - (void)one; - return 1; - }; + (void)one; + return 1; + }; auto lambda_two_ints = [](int one, int two) { - (void)one; - (void)two; - return 2; - }; + (void)one; + (void)two; + return 2; + }; auto lambda_one_int_one_char = [](int one, char two) { - (void)one; - (void)two; - return 3; - }; + (void)one; + (void)two; + return 3; + }; static_assert( rclcpp::function_traits::function_traits::arity == 0, @@ -266,232 +248,243 @@ TEST(TestFunctionTraits, arity) { /* Tests that funcion_traits deducts the type of the arguments of several functors. */ -TEST(TestFunctionTraits, argument_types) { +TEST(TestFunctionTraits, argument_types) +{ // Test regular functions static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts an int as first argument"); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts an int as first argument"); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<1> - >::value, "Functor accepts an int as second argument"); + rclcpp::function_traits::function_traits::template argument_type<1> >::value, + "Functor accepts an int as second argument"); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits< - decltype(func_one_int_one_char) - >::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts an int as first argument"); static_assert( std::is_same< char, - rclcpp::function_traits::function_traits< - decltype(func_one_int_one_char) - >::template argument_type<1> - >::value, "Functor accepts a char as second argument"); + rclcpp::function_traits::function_traits::template argument_type<1> >::value, + "Functor accepts a char as second argument"); // Test lambdas auto lambda_one_int = [](int one) { - (void)one; - return 1; - }; + (void)one; + return 1; + }; auto lambda_two_ints = [](int one, int two) { - (void)one; - (void)two; - return 2; - }; + (void)one; + (void)two; + return 2; + }; auto lambda_one_int_one_char = [](int one, char two) { - (void)one; - (void)two; - return 3; - }; + (void)one; + (void)two; + return 3; + }; static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts an int as first argument"); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts an int as first argument"); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<1> - >::value, "Functor accepts an int as second argument"); + rclcpp::function_traits::function_traits::template argument_type<1> >::value, + "Functor accepts an int as second argument"); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits< - decltype(lambda_one_int_one_char) - >::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts an int as first argument"); static_assert( std::is_same< char, - rclcpp::function_traits::function_traits< - decltype(lambda_one_int_one_char) - >::template argument_type<1> - >::value, "Functor accepts a char as second argument"); + rclcpp::function_traits::function_traits::template argument_type<1> >::value, + "Functor accepts a char as second argument"); // Test objects that have a call operator static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >:: + value, + "Functor accepts an int as first argument"); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >:: + value, + "Functor accepts an int as first argument"); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<1> - >::value, "Functor accepts an int as second argument"); + rclcpp::function_traits::function_traits::template argument_type<1> >:: + value, + "Functor accepts an int as second argument"); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits< - FunctionObjectOneIntOneChar - >::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type< + 0> >::value, + "Functor accepts an int as first argument"); static_assert( std::is_same< char, - rclcpp::function_traits::function_traits< - FunctionObjectOneIntOneChar - >::template argument_type<1> - >::value, "Functor accepts a char as second argument"); + rclcpp::function_traits::function_traits::template argument_type< + 1> >::value, + "Functor accepts a char as second argument"); ObjectMember object_member; - auto bind_one_bool = std::bind( - &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1); + auto bind_one_bool = + std::bind(&ObjectMember::callback_one_bool, &object_member, std::placeholders::_1); static_assert( std::is_same< bool, - rclcpp::function_traits::function_traits::template argument_type<0> - >::value, "Functor accepts a bool as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts a bool as first argument"); - auto bind_one_bool_const = std::bind( - &ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1); + auto bind_one_bool_const = + std::bind(&ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1); static_assert( std::is_same< bool, - rclcpp::function_traits::function_traits::template - argument_type<0> - >::value, "Functor accepts a bool as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts a bool as first argument"); auto bind_two_bools = std::bind( - &ObjectMember::callback_two_bools, &object_member, std::placeholders::_1, + &ObjectMember::callback_two_bools, + &object_member, + std::placeholders::_1, std::placeholders::_2); static_assert( std::is_same< bool, - rclcpp::function_traits::function_traits::template argument_type<0> - >::value, "Functor accepts a bool as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts a bool as first argument"); static_assert( std::is_same< bool, - rclcpp::function_traits::function_traits::template argument_type<1> - >::value, "Functor accepts a bool as second argument"); + rclcpp::function_traits::function_traits::template argument_type<1> >::value, + "Functor accepts a bool as second argument"); auto bind_one_bool_one_float = std::bind( - &ObjectMember::callback_one_bool_one_float, &object_member, std::placeholders::_1, + &ObjectMember::callback_one_bool_one_float, + &object_member, + std::placeholders::_1, std::placeholders::_2); static_assert( std::is_same< bool, - rclcpp::function_traits::function_traits< - decltype(bind_one_bool_one_float) - >::template argument_type<0> - >::value, "Functor accepts a bool as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts a bool as first argument"); static_assert( std::is_same< float, - rclcpp::function_traits::function_traits< - decltype(bind_one_bool_one_float) - >::template argument_type<1> - >::value, "Functor accepts a float as second argument"); + rclcpp::function_traits::function_traits::template argument_type<1> >::value, + "Functor accepts a float as second argument"); auto bind_one_int = std::bind(func_one_int, std::placeholders::_1); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts an int as first argument"); auto bind_two_ints = std::bind(func_two_ints, std::placeholders::_1, std::placeholders::_2); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts an int as first argument"); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits::template argument_type<1> - >::value, "Functor accepts an int as second argument"); + rclcpp::function_traits::function_traits::template argument_type<1> >::value, + "Functor accepts an int as second argument"); - auto bind_one_int_one_char = std::bind( - func_one_int_one_char, std::placeholders::_1, std::placeholders::_2); + auto bind_one_int_one_char = + std::bind(func_one_int_one_char, std::placeholders::_1, std::placeholders::_2); static_assert( std::is_same< int, - rclcpp::function_traits::function_traits< - decltype(bind_one_int_one_char) - >::template argument_type<0> - >::value, "Functor accepts an int as first argument"); + rclcpp::function_traits::function_traits::template argument_type<0> >::value, + "Functor accepts an int as first argument"); static_assert( std::is_same< char, - rclcpp::function_traits::function_traits< - decltype(bind_one_int_one_char) - >::template argument_type<1> - >::value, "Functor accepts a char as second argument"); + rclcpp::function_traits::function_traits::template argument_type<1> >::value, + "Functor accepts a char as second argument"); } /* Tests that funcion_traits checks the types of the arguments of several functors. */ -TEST(TestFunctionTraits, check_arguments) { +TEST(TestFunctionTraits, check_arguments) +{ // Test regular functions static_assert( rclcpp::function_traits::check_arguments::value, @@ -527,21 +520,21 @@ TEST(TestFunctionTraits, check_arguments) { // Test lambdas auto lambda_one_int = [](int one) { - (void)one; - return 1; - }; + (void)one; + return 1; + }; auto lambda_two_ints = [](int one, int two) { - (void)one; - (void)two; - return 2; - }; + (void)one; + (void)two; + return 2; + }; auto lambda_one_int_one_char = [](int one, char two) { - (void)one; - (void)two; - return 3; - }; + (void)one; + (void)two; + return 3; + }; static_assert( rclcpp::function_traits::check_arguments::value, @@ -570,16 +563,16 @@ TEST(TestFunctionTraits, check_arguments) { ObjectMember object_member; - auto bind_one_bool = std::bind( - &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1); + auto bind_one_bool = + std::bind(&ObjectMember::callback_one_bool, &object_member, std::placeholders::_1); // Test std::bind functions static_assert( rclcpp::function_traits::check_arguments::value, "Functor accepts a single bool as arguments"); - auto bind_one_bool_const = std::bind( - &ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1); + auto bind_one_bool_const = + std::bind(&ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1); // Test std::bind functions static_assert( @@ -590,40 +583,37 @@ TEST(TestFunctionTraits, check_arguments) { /* Tests that same_arguments work. */ -TEST(TestFunctionTraits, same_arguments) { +TEST(TestFunctionTraits, same_arguments) +{ auto lambda_one_int = [](int one) { - (void)one; - return 1; - }; + (void)one; + return 1; + }; auto lambda_two_ints = [](int one, int two) { - (void)one; - (void)two; - return 1; - }; + (void)one; + (void)two; + return 1; + }; static_assert( - rclcpp::function_traits::same_arguments< - decltype(lambda_one_int), decltype(func_one_int) - >::value, + rclcpp::function_traits::same_arguments:: + value, "Lambda and function have the same arguments"); static_assert( - !rclcpp::function_traits::same_arguments< - decltype(lambda_two_ints), decltype(func_one_int) - >::value, + !rclcpp::function_traits::same_arguments:: + value, "Lambda and function have different arguments"); static_assert( - !rclcpp::function_traits::same_arguments< - decltype(func_one_int_one_char), decltype(func_two_ints) - >::value, + !rclcpp::function_traits:: + same_arguments::value, "Functions have different arguments"); static_assert( - !rclcpp::function_traits::same_arguments< - decltype(lambda_one_int), decltype(lambda_two_ints) - >::value, + !rclcpp::function_traits::same_arguments:: + value, "Lambdas have different arguments"); static_assert( @@ -631,48 +621,44 @@ TEST(TestFunctionTraits, same_arguments) { "Functor and function have the same arguments"); static_assert( - rclcpp::function_traits::same_arguments< - FunctionObjectTwoInts, decltype(lambda_two_ints)>::value, + rclcpp::function_traits::same_arguments:: + value, "Functor and lambda have the same arguments"); } -TEST(TestFunctionTraits, return_type) { +TEST(TestFunctionTraits, return_type) +{ // Test regular function static_assert( std::is_same< rclcpp::function_traits::function_traits::return_type, - int - >::value, + int>::value, "Functor return ints"); // Test lambda auto lambda_one_int_return_double = [](int one) -> double { - (void)one; - return 1.0; - }; + (void)one; + return 1.0; + }; static_assert( std::is_same< - rclcpp::function_traits::function_traits< - decltype(lambda_one_int_return_double) - >::return_type, - double - >::value, + rclcpp::function_traits::function_traits::return_type, + double>::value, "Lambda returns a double"); // Test objects that have a call operator static_assert( - std::is_same< - rclcpp::function_traits::function_traits::return_type, - int - >::value, + std::is_same::return_type, int>:: + value, "Functor return ints"); } /* Tests that functions are matched via SFINAE. */ -TEST(TestFunctionTraits, sfinae_match) { +TEST(TestFunctionTraits, sfinae_match) +{ EXPECT_EQ(0, func_accept_callback(func_no_args)); EXPECT_EQ(1, func_accept_callback(func_one_int)); @@ -681,26 +667,24 @@ TEST(TestFunctionTraits, sfinae_match) { EXPECT_EQ(3, func_accept_callback(func_one_int_one_char)); - auto lambda_no_args = []() { - return 0; - }; + auto lambda_no_args = []() { return 0; }; auto lambda_one_int = [](int one) { - (void)one; - return 1; - }; + (void)one; + return 1; + }; auto lambda_two_ints = [](int one, int two) { - (void)one; - (void)two; - return 2; - }; + (void)one; + (void)two; + return 2; + }; auto lambda_one_int_one_char = [](int one, char two) { - (void)one; - (void)two; - return 3; - }; + (void)one; + (void)two; + return 3; + }; EXPECT_EQ(0, func_accept_callback(lambda_no_args)); @@ -718,13 +702,9 @@ TEST(TestFunctionTraits, sfinae_match) { EXPECT_EQ(3, func_accept_callback(FunctionObjectOneIntOneChar())); - auto lambda_no_args_double = []() -> double { - return 123.45; - }; + auto lambda_no_args_double = []() -> double { return 123.45; }; - auto lambda_no_args_string = []() -> std::string { - return std::string("foo"); - }; + auto lambda_no_args_string = []() -> std::string { return std::string("foo"); }; EXPECT_EQ(123.45, func_accept_callback_return_type(lambda_no_args_double)); @@ -734,20 +714,26 @@ TEST(TestFunctionTraits, sfinae_match) { class TestMember : public ::testing::Test { public: - void MemberFunctor(int, float, std::string) {} + void MemberFunctor(int, float, std::string) + { + } }; /* Regression test for https://github.com/ros2/rclcpp/issues/479, specific to classes using the TEST_F GTest macro. */ -TEST_F(TestMember, bind_member_functor) { +TEST_F(TestMember, bind_member_functor) +{ auto bind_member_functor = std::bind( - &TestMember::MemberFunctor, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3); + &TestMember::MemberFunctor, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3); static_assert( - rclcpp::function_traits::check_arguments::value, + rclcpp::function_traits:: + check_arguments::value, "Functor accepts an int, a float and a string as arguments"); } diff --git a/rclcpp/test/test_init.cpp b/rclcpp/test/test_init.cpp index 78554993d8..48be1dadea 100644 --- a/rclcpp/test/test_init.cpp +++ b/rclcpp/test/test_init.cpp @@ -16,7 +16,8 @@ #include "rclcpp/utilities.hpp" -TEST(TestInit, is_initialized) { +TEST(TestInit, is_initialized) +{ EXPECT_FALSE(rclcpp::is_initialized()); rclcpp::init(0, nullptr); diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index fe3c535107..587bd51554 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -34,11 +34,13 @@ class PublisherBase public: RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) - PublisherBase() - : mock_topic_name(""), mock_queue_size(0) {} + PublisherBase() : mock_topic_name(""), mock_queue_size(0) + { + } virtual ~PublisherBase() - {} + { + } const char * get_topic_name() const { @@ -49,16 +51,14 @@ class PublisherBase return mock_queue_size; } - bool - operator==(const rmw_gid_t * gid) const + bool operator==(const rmw_gid_t * gid) const { (void)gid; return false; } - virtual - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - make_mapped_ring_buffer(size_t size) const + virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr make_mapped_ring_buffer( + size_t size) const { (void)size; return nullptr; @@ -85,13 +85,11 @@ class Publisher : public PublisherBase allocator_ = std::make_shared(); } - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - make_mapped_ring_buffer(size_t size) const override + mapped_ring_buffer::MappedRingBufferBase::SharedPtr make_mapped_ring_buffer( + size_t size) const override { - return mapped_ring_buffer::MappedRingBuffer< - T, - typename Publisher::MessageAlloc - >::make_shared(size, allocator_); + return mapped_ring_buffer::MappedRingBuffer::MessageAlloc>:: + make_shared(size, allocator_); } std::shared_ptr get_allocator() @@ -113,8 +111,9 @@ class SubscriptionBase public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionBase) - SubscriptionBase() - : mock_topic_name(""), mock_queue_size(0) {} + SubscriptionBase() : mock_topic_name(""), mock_queue_size(0) + { + } const char * get_topic_name() const { @@ -158,18 +157,15 @@ class SubscriptionBase - Asserts the message it got back was the one that went in (since there's only one subscription). - Try's to take the message again, should fail. */ -TEST(TestIntraProcessManager, nominal) { +TEST(TestIntraProcessManager, nominal) +{ rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p1 = std::make_shared>(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 2; - auto p2 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p2 = std::make_shared>(); p2->mock_topic_name = "nominal2"; p2->mock_queue_size = 10; @@ -185,8 +181,7 @@ TEST(TestIntraProcessManager, nominal) { ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); auto p1_m1_original_address = unique_msg.get(); auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); @@ -243,12 +238,11 @@ TEST(TestIntraProcessManager, nominal) { - Remove the publisher. - Try's to take the message, should fail since the publisher (and its storage) is gone. */ -TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) { +TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) +{ rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p1 = std::make_shared>(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 10; @@ -263,8 +257,7 @@ TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) { ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); ASSERT_EQ(nullptr, unique_msg); @@ -284,12 +277,11 @@ TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) { - Take with the final subscription, should work. - Assert the previous take returned ownership of the original object published. */ -TEST(TestIntraProcessManager, removed_subscription_affects_take) { +TEST(TestIntraProcessManager, removed_subscription_affects_take) +{ rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p1 = std::make_shared>(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 10; @@ -314,8 +306,7 @@ TEST(TestIntraProcessManager, removed_subscription_affects_take) { ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); auto original_message_pointer = unique_msg.get(); auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); @@ -354,12 +345,11 @@ TEST(TestIntraProcessManager, removed_subscription_affects_take) { - Publish a message. - Take with each subscription, checking that the last takes the original back. */ -TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) { +TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) +{ rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p1 = std::make_shared>(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 10; @@ -384,8 +374,7 @@ TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) { ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); auto original_message_pointer = unique_msg.get(); auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); @@ -425,24 +414,19 @@ TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) { - Publish a message. - Take with each subscription, checking that the last takes the original back. */ -TEST(TestIntraProcessManager, multiple_publishers_one_subscription) { +TEST(TestIntraProcessManager, multiple_publishers_one_subscription) +{ rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p1 = std::make_shared>(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 10; - auto p2 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p2 = std::make_shared>(); p2->mock_topic_name = "nominal1"; p2->mock_queue_size = 10; - auto p3 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p3 = std::make_shared>(); p3->mock_topic_name = "nominal1"; p3->mock_queue_size = 10; @@ -460,8 +444,7 @@ TEST(TestIntraProcessManager, multiple_publishers_one_subscription) { ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); auto original_message_pointer1 = unique_msg.get(); auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); @@ -522,24 +505,19 @@ TEST(TestIntraProcessManager, multiple_publishers_one_subscription) { - Publish a message on each publisher. - Take from each publisher with each subscription, checking the pointer. */ -TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) { +TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) +{ rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p1 = std::make_shared>(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 10; - auto p2 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p2 = std::make_shared>(); p2->mock_topic_name = "nominal1"; p2->mock_queue_size = 10; - auto p3 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p3 = std::make_shared>(); p3->mock_topic_name = "nominal1"; p3->mock_queue_size = 10; @@ -567,8 +545,7 @@ TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) { ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); auto original_message_pointer1 = unique_msg.get(); auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); @@ -686,12 +663,11 @@ TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) { - Publish a message. - Try to take the first message, should fail. */ -TEST(TestIntraProcessManager, ring_buffer_displacement) { +TEST(TestIntraProcessManager, ring_buffer_displacement) +{ rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p1 = std::make_shared>(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 2; @@ -706,8 +682,7 @@ TEST(TestIntraProcessManager, ring_buffer_displacement) { ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); ASSERT_EQ(nullptr, unique_msg); @@ -749,12 +724,11 @@ TEST(TestIntraProcessManager, ring_buffer_displacement) { - Create a subscription on the same topic. - Try to take the message with the newly created subscription, should fail. */ -TEST(TestIntraProcessManager, subscription_creation_race_condition) { +TEST(TestIntraProcessManager, subscription_creation_race_condition) +{ rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p1 = std::make_shared>(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 2; @@ -764,8 +738,7 @@ TEST(TestIntraProcessManager, subscription_creation_race_condition) { ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); ASSERT_EQ(nullptr, unique_msg); @@ -788,7 +761,8 @@ TEST(TestIntraProcessManager, subscription_creation_race_condition) { - Let the scope expire. - Try to take the message with the subscription, should fail. */ -TEST(TestIntraProcessManager, publisher_out_of_scope_take) { +TEST(TestIntraProcessManager, publisher_out_of_scope_take) +{ rclcpp::intra_process_manager::IntraProcessManager ipm; auto s1 = std::make_shared(); @@ -800,9 +774,7 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_take) { uint64_t p1_id; uint64_t p1_m1_id; { - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p1 = std::make_shared>(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 2; @@ -812,8 +784,7 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_take) { ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); ASSERT_EQ(nullptr, unique_msg); @@ -834,14 +805,13 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_take) { - Let the scope expire. - Publish a message on the publisher in a scope, should throw. */ -TEST(TestIntraProcessManager, publisher_out_of_scope_store) { +TEST(TestIntraProcessManager, publisher_out_of_scope_store) +{ rclcpp::intra_process_manager::IntraProcessManager ipm; uint64_t p1_id; { - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); + auto p1 = std::make_shared>(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 2; @@ -852,8 +822,7 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_store) { ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); EXPECT_THROW(ipm.store_intra_process_message(p1_id, std::move(unique_msg)), std::runtime_error); ASSERT_EQ(nullptr, unique_msg); diff --git a/rclcpp/test/test_logger.cpp b/rclcpp/test/test_logger.cpp index 258cdd0959..d167b8919f 100644 --- a/rclcpp/test/test_logger.cpp +++ b/rclcpp/test/test_logger.cpp @@ -19,14 +19,16 @@ #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" -TEST(TestLogger, factory_functions) { +TEST(TestLogger, factory_functions) +{ rclcpp::Logger logger = rclcpp::get_logger("test_logger"); EXPECT_STREQ("test_logger", logger.get_name()); rclcpp::Logger logger_copy = rclcpp::Logger(logger); EXPECT_STREQ("test_logger", logger_copy.get_name()); } -TEST(TestLogger, hierarchy) { +TEST(TestLogger, hierarchy) +{ rclcpp::Logger logger = rclcpp::get_logger("test_logger"); rclcpp::Logger sublogger = logger.get_child("child"); EXPECT_STREQ("test_logger.child", sublogger.get_name()); diff --git a/rclcpp/test/test_logging.cpp b/rclcpp/test/test_logging.cpp index 992ef01209..40295b7c69 100644 --- a/rclcpp/test/test_logging.cpp +++ b/rclcpp/test/test_logging.cpp @@ -50,19 +50,21 @@ class TestLoggingMacros : public ::testing::Test rcutils_logging_set_default_logger_level(RCUTILS_LOG_SEVERITY_DEBUG); auto rcutils_logging_console_output_handler = []( - const rcutils_log_location_t * location, - int level, const char * name, rcutils_time_point_value_t timestamp, - const char * format, va_list * args) -> void - { - g_log_calls += 1; - g_last_log_event.location = location; - g_last_log_event.level = level; - g_last_log_event.name = name ? name : ""; - g_last_log_event.timestamp = timestamp; - char buffer[1024]; - vsnprintf(buffer, sizeof(buffer), format, *args); - g_last_log_event.message = buffer; - }; + const rcutils_log_location_t * location, + int level, + const char * name, + rcutils_time_point_value_t timestamp, + const char * format, + va_list * args) -> void { + g_log_calls += 1; + g_last_log_event.location = location; + g_last_log_event.level = level; + g_last_log_event.name = name ? name : ""; + g_last_log_event.timestamp = timestamp; + char buffer[1024]; + vsnprintf(buffer, sizeof(buffer), format, *args); + g_last_log_event.message = buffer; + }; this->previous_output_handler = rcutils_logging_get_output_handler(); rcutils_logging_set_output_handler(rcutils_logging_console_output_handler); @@ -76,7 +78,8 @@ class TestLoggingMacros : public ::testing::Test } }; -TEST_F(TestLoggingMacros, test_logging_named) { +TEST_F(TestLoggingMacros, test_logging_named) +{ for (int i : {1, 2, 3}) { RCLCPP_DEBUG(g_logger, "message %d", i); } @@ -92,17 +95,24 @@ TEST_F(TestLoggingMacros, test_logging_named) { EXPECT_EQ("message 3", g_last_log_event.message); } -TEST_F(TestLoggingMacros, test_logging_string) { +TEST_F(TestLoggingMacros, test_logging_string) +{ for (std::string i : {"one", "two", "three"}) { RCLCPP_DEBUG(g_logger, "message " + i); } EXPECT_EQ(3u, g_log_calls); EXPECT_EQ("message three", g_last_log_event.message); - RCLCPP_DEBUG(g_logger, "message " "four"); + RCLCPP_DEBUG( + g_logger, + "message " + "four"); EXPECT_EQ("message four", g_last_log_event.message); - RCLCPP_DEBUG(g_logger, std::string("message " "five")); + RCLCPP_DEBUG( + g_logger, + std::string("message " + "five")); EXPECT_EQ("message five", g_last_log_event.message); RCLCPP_DEBUG(g_logger, std::string("message %s"), "six"); @@ -112,7 +122,8 @@ TEST_F(TestLoggingMacros, test_logging_string) { EXPECT_EQ("message seven", g_last_log_event.message); } -TEST_F(TestLoggingMacros, test_logging_once) { +TEST_F(TestLoggingMacros, test_logging_once) +{ for (int i : {1, 2, 3}) { RCLCPP_INFO_ONCE(g_logger, "message %d", i); } @@ -132,7 +143,8 @@ TEST_F(TestLoggingMacros, test_logging_once) { EXPECT_EQ("second message 1", g_last_log_event.message); } -TEST_F(TestLoggingMacros, test_logging_expression) { +TEST_F(TestLoggingMacros, test_logging_expression) +{ for (int i : {1, 2, 3, 4, 5, 6}) { RCLCPP_INFO_EXPRESSION(g_logger, i % 3, "message %d", i); } @@ -147,7 +159,8 @@ bool mod3() return (g_counter % 3) != 0; } -TEST_F(TestLoggingMacros, test_logging_function) { +TEST_F(TestLoggingMacros, test_logging_function) +{ for (int i : {1, 2, 3, 4, 5, 6}) { g_counter = i; RCLCPP_INFO_FUNCTION(g_logger, &mod3, "message %d", i); @@ -156,7 +169,8 @@ TEST_F(TestLoggingMacros, test_logging_function) { EXPECT_EQ("message 5", g_last_log_event.message); } -TEST_F(TestLoggingMacros, test_logging_skipfirst) { +TEST_F(TestLoggingMacros, test_logging_skipfirst) +{ for (uint32_t i : {1, 2, 3, 4, 5}) { RCLCPP_WARN_SKIPFIRST(g_logger, "message %u", i); EXPECT_EQ(i - 1, g_log_calls); @@ -181,7 +195,8 @@ bool log_function_const_ref(const rclcpp::Logger & logger) return true; } -TEST_F(TestLoggingMacros, test_log_from_node) { +TEST_F(TestLoggingMacros, test_log_from_node) +{ auto logger = rclcpp::get_logger("test_logging_logger"); EXPECT_TRUE(log_function(logger)); EXPECT_TRUE(log_function_const(logger)); diff --git a/rclcpp/test/test_mapped_ring_buffer.cpp b/rclcpp/test/test_mapped_ring_buffer.cpp index 5625804d13..a0da81b9eb 100644 --- a/rclcpp/test/test_mapped_ring_buffer.cpp +++ b/rclcpp/test/test_mapped_ring_buffer.cpp @@ -23,7 +23,8 @@ /* Tests get_copy and pop on an empty mrb. */ -TEST(TestMappedRingBuffer, empty) { +TEST(TestMappedRingBuffer, empty) +{ // Cannot create a buffer of size zero. EXPECT_THROW(rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(0), std::invalid_argument); // Getting or popping an empty buffer should result in a nullptr. @@ -48,7 +49,8 @@ TEST(TestMappedRingBuffer, empty) { Tests push_and_replace with a temporary object, and using get and pop methods with shared_ptr signature. */ -TEST(TestMappedRingBuffer, temporary_l_value_with_shared_get_pop) { +TEST(TestMappedRingBuffer, temporary_l_value_with_shared_get_pop) +{ rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); // Pass in value with temporary object mrb.push_and_replace(1, std::shared_ptr(new char('a'))); @@ -68,7 +70,8 @@ TEST(TestMappedRingBuffer, temporary_l_value_with_shared_get_pop) { Tests push_and_replace with a temporary object, and using get and pop methods with unique_ptr signature. */ -TEST(TestMappedRingBuffer, temporary_l_value_with_unique_get_pop) { +TEST(TestMappedRingBuffer, temporary_l_value_with_unique_get_pop) +{ rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); // Pass in value with temporary object mrb.push_and_replace(1, std::shared_ptr(new char('a'))); @@ -88,7 +91,8 @@ TEST(TestMappedRingBuffer, temporary_l_value_with_unique_get_pop) { Tests normal usage of the mrb. Using shared push_and_replace, get and pop methods. */ -TEST(TestMappedRingBuffer, nominal_push_shared_get_pop_shared) { +TEST(TestMappedRingBuffer, nominal_push_shared_get_pop_shared) +{ rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); std::shared_ptr expected(new char('a')); @@ -144,7 +148,8 @@ TEST(TestMappedRingBuffer, nominal_push_shared_get_pop_shared) { Tests normal usage of the mrb. Using shared push_and_replace, unique get and pop methods. */ -TEST(TestMappedRingBuffer, nominal_push_shared_get_pop_unique) { +TEST(TestMappedRingBuffer, nominal_push_shared_get_pop_unique) +{ rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); std::shared_ptr expected(new char('a')); const char * expected_orig = expected.get(); @@ -206,7 +211,8 @@ TEST(TestMappedRingBuffer, nominal_push_shared_get_pop_unique) { Tests normal usage of the mrb. Using unique push_and_replace, get and pop methods. */ -TEST(TestMappedRingBuffer, nominal_push_unique_get_pop_unique) { +TEST(TestMappedRingBuffer, nominal_push_unique_get_pop_unique) +{ rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); std::unique_ptr expected(new char('a')); const char * expected_orig = expected.get(); @@ -257,7 +263,8 @@ TEST(TestMappedRingBuffer, nominal_push_unique_get_pop_unique) { Tests normal usage of the mrb. Using unique push_and_replace, shared get and pop methods. */ -TEST(TestMappedRingBuffer, nominal_push_unique_get_pop_shared) { +TEST(TestMappedRingBuffer, nominal_push_unique_get_pop_shared) +{ rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); std::unique_ptr expected(new char('a')); const char * expected_orig = expected.get(); @@ -307,7 +314,8 @@ TEST(TestMappedRingBuffer, nominal_push_unique_get_pop_shared) { /* Tests the affect of reusing keys (non-unique keys) in a mrb. */ -TEST(TestMappedRingBuffer, non_unique_keys) { +TEST(TestMappedRingBuffer, non_unique_keys) +{ rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); std::shared_ptr input(new char('a')); diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp index 583a697593..8ff2baffc2 100644 --- a/rclcpp/test/test_node.cpp +++ b/rclcpp/test/test_node.cpp @@ -22,8 +22,8 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/scope_exit.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/scope_exit.hpp" class TestNode : public ::testing::Test { @@ -37,27 +37,27 @@ class TestNode : public ::testing::Test /* Testing node construction and destruction. */ -TEST_F(TestNode, construction_and_destruction) { +TEST_F(TestNode, construction_and_destruction) +{ { std::make_shared("my_node", "/ns"); } { ASSERT_THROW( - { - std::make_shared("invalid_node?", "/ns"); - }, rclcpp::exceptions::InvalidNodeNameError); + { std::make_shared("invalid_node?", "/ns"); }, + rclcpp::exceptions::InvalidNodeNameError); } { ASSERT_THROW( - { - std::make_shared("my_node", "/invalid_ns?"); - }, rclcpp::exceptions::InvalidNamespaceError); + { std::make_shared("my_node", "/invalid_ns?"); }, + rclcpp::exceptions::InvalidNamespaceError); } } -TEST_F(TestNode, get_name_and_namespace) { +TEST_F(TestNode, get_name_and_namespace) +{ { auto node = std::make_shared("my_node", "/ns"); EXPECT_STREQ("my_node", node->get_name()); @@ -65,8 +65,7 @@ TEST_F(TestNode, get_name_and_namespace) { EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name()); } { - auto options = rclcpp::NodeOptions() - .arguments({"--ros-args", "-r", "__ns:=/another_ns"}); + auto options = rclcpp::NodeOptions().arguments({"--ros-args", "-r", "__ns:=/another_ns"}); auto node = std::make_shared("my_node", "/ns", options); EXPECT_STREQ("my_node", node->get_name()); EXPECT_STREQ("/another_ns", node->get_namespace()); @@ -108,13 +107,11 @@ TEST_F(TestNode, get_name_and_namespace) { auto node3 = std::make_shared("my_node3", "/ns2"); auto node4 = std::make_shared("my_node4", "my/ns3"); auto names_and_namespaces = node1->get_node_names(); - auto name_namespace_set = std::unordered_set( - names_and_namespaces.begin(), - names_and_namespaces.end()); - std::function Set_Contains = [&](std::string string_key) - { - return name_namespace_set.find(string_key) != name_namespace_set.end(); - }; + auto name_namespace_set = + std::unordered_set(names_and_namespaces.begin(), names_and_namespaces.end()); + std::function Set_Contains = [&](std::string string_key) { + return name_namespace_set.find(string_key) != name_namespace_set.end(); + }; EXPECT_TRUE(Set_Contains("/my/ns/my_node1")); EXPECT_TRUE(Set_Contains("/my/ns/my_node2")); EXPECT_TRUE(Set_Contains("/ns2/my_node3")); @@ -122,7 +119,8 @@ TEST_F(TestNode, get_name_and_namespace) { } } -TEST_F(TestNode, subnode_get_name_and_namespace) { +TEST_F(TestNode, subnode_get_name_and_namespace) +{ { auto node = std::make_shared("my_node", "ns"); auto subnode = node->create_sub_node("sub_ns"); @@ -176,87 +174,92 @@ TEST_F(TestNode, subnode_get_name_and_namespace) { { auto node = std::make_shared("my_node"); ASSERT_THROW( - { - auto subnode = node->create_sub_node("/sub_ns"); - }, rclcpp::exceptions::NameValidationError); + { auto subnode = node->create_sub_node("/sub_ns"); }, + rclcpp::exceptions::NameValidationError); } } /* Testing node construction and destruction. */ -TEST_F(TestNode, subnode_construction_and_destruction) { +TEST_F(TestNode, subnode_construction_and_destruction) +{ { - ASSERT_NO_THROW( - { + ASSERT_NO_THROW({ auto node = std::make_shared("my_node", "ns"); auto subnode = node->create_sub_node("sub_ns"); }); } { ASSERT_THROW( - { - auto node = std::make_shared("my_node", "ns"); - auto subnode = node->create_sub_node("invalid_ns?"); - }, rclcpp::exceptions::InvalidNamespaceError); + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("invalid_ns?"); + }, + rclcpp::exceptions::InvalidNamespaceError); } { ASSERT_THROW( - { - auto node = std::make_shared("my_node", "ns/"); - }, rclcpp::exceptions::InvalidNamespaceError); + { auto node = std::make_shared("my_node", "ns/"); }, + rclcpp::exceptions::InvalidNamespaceError); } { ASSERT_THROW( - { - auto node = std::make_shared("my_node", "ns/"); - auto subnode = node->create_sub_node("/sub_ns"); - }, rclcpp::exceptions::InvalidNamespaceError); + { + auto node = std::make_shared("my_node", "ns/"); + auto subnode = node->create_sub_node("/sub_ns"); + }, + rclcpp::exceptions::InvalidNamespaceError); } { ASSERT_THROW( - { - auto node = std::make_shared("my_node", "ns"); - auto subnode = node->create_sub_node("/sub_ns"); - }, rclcpp::exceptions::NameValidationError); + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("/sub_ns"); + }, + rclcpp::exceptions::NameValidationError); } { ASSERT_THROW( - { - auto node = std::make_shared("my_node", "ns"); - auto subnode = node->create_sub_node("~sub_ns"); - }, rclcpp::exceptions::InvalidNamespaceError); + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("~sub_ns"); + }, + rclcpp::exceptions::InvalidNamespaceError); } { ASSERT_THROW( - { - auto node = std::make_shared("my_node", "/ns"); - auto subnode = node->create_sub_node("invalid_ns?"); - }, rclcpp::exceptions::InvalidNamespaceError); + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("invalid_ns?"); + }, + rclcpp::exceptions::InvalidNamespaceError); } { - ASSERT_NO_THROW( - { + ASSERT_NO_THROW({ auto node = std::make_shared("my_node", "/ns"); auto subnode = node->create_sub_node("sub_ns"); }); } { ASSERT_THROW( - { - auto node = std::make_shared("my_node", "/ns"); - auto subnode = node->create_sub_node("/sub_ns"); - }, rclcpp::exceptions::NameValidationError); + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("/sub_ns"); + }, + rclcpp::exceptions::NameValidationError); } { ASSERT_THROW( - { - auto node = std::make_shared("my_node", "/ns"); - auto subnode = node->create_sub_node("~sub_ns"); - }, rclcpp::exceptions::InvalidNamespaceError); + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("~sub_ns"); + }, + rclcpp::exceptions::InvalidNamespaceError); } } -TEST_F(TestNode, get_logger) { +TEST_F(TestNode, get_logger) +{ { auto node = std::make_shared("my_node"); EXPECT_STREQ("my_node", node->get_logger().get_name()); @@ -279,14 +282,16 @@ TEST_F(TestNode, get_logger) { } } -TEST_F(TestNode, get_clock) { +TEST_F(TestNode, get_clock) +{ auto node = std::make_shared("my_node", "/ns"); auto ros_clock = node->get_clock(); EXPECT_TRUE(ros_clock != nullptr); EXPECT_EQ(ros_clock->get_clock_type(), RCL_ROS_TIME); } -TEST_F(TestNode, now) { +TEST_F(TestNode, now) +{ auto node = std::make_shared("my_node", "/ns"); auto clock = node->get_clock(); auto now_builtin = node->now().nanoseconds(); @@ -295,14 +300,14 @@ TEST_F(TestNode, now) { EXPECT_LT(now_external - now_builtin, 5000000L); } -std::string -operator"" _unq(const char * prefix, size_t prefix_length) +std::string operator"" _unq(const char * prefix, size_t prefix_length) { static uint64_t count = 0; return std::string(prefix, prefix_length) + "_" + std::to_string(++count); } -TEST_F(TestNode, declare_parameter_with_no_initial_values) { +TEST_F(TestNode, declare_parameter_with_no_initial_values) +{ // test cases without initial values auto node = std::make_shared("test_declare_parameter_node"_unq); { @@ -323,12 +328,10 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) { rclcpp::ParameterValue default_value(42); rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.read_only = true; - rclcpp::ParameterValue value = - node->declare_parameter(name, default_value, descriptor); + rclcpp::ParameterValue value = node->declare_parameter(name, default_value, descriptor); EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); EXPECT_EQ(value.get(), default_value.get()); - rcl_interfaces::msg::ParameterDescriptor actual_descriptor = - node->describe_parameter(name); + rcl_interfaces::msg::ParameterDescriptor actual_descriptor = node->describe_parameter(name); EXPECT_EQ(actual_descriptor.read_only, descriptor.read_only); } { @@ -341,69 +344,62 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) { auto name = "parameter"_unq; node->declare_parameter(name); EXPECT_THROW( - {node->declare_parameter(name);}, - rclcpp::exceptions::ParameterAlreadyDeclaredException); + { node->declare_parameter(name); }, rclcpp::exceptions::ParameterAlreadyDeclaredException); } { // parameter name invalid throws - EXPECT_THROW( - {node->declare_parameter("");}, - rclcpp::exceptions::InvalidParametersException); + EXPECT_THROW({ node->declare_parameter(""); }, rclcpp::exceptions::InvalidParametersException); } { // parameter rejected throws - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset auto name = "parameter"_unq; - auto on_set_parameters = - [&name](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - for (const auto & parameter : parameters) { - if ( - parameter.get_name() == name && - parameter.get_type() != rclcpp::PARAMETER_INTEGER) - { - result.successful = false; - result.reason = "'" + name + "' must be an integer"; - } + auto on_set_parameters = [&name](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if (parameter.get_name() == name && parameter.get_type() != rclcpp::PARAMETER_INTEGER) { + result.successful = false; + result.reason = "'" + name + "' must be an integer"; } - return result; - }; + } + return result; + }; EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); EXPECT_THROW( - {node->declare_parameter(name, "not an int");}, + { node->declare_parameter(name, "not an int"); }, rclcpp::exceptions::InvalidParameterValueException); } } auto get_fixed_on_parameter_set_callback(const std::string & name, bool successful) { - return - [name, successful](const std::vector & parameters) { - (void)parameters; - rcl_interfaces::msg::SetParametersResult result; - result.successful = successful; - return result; - }; + return [name, successful](const std::vector & parameters) { + (void)parameters; + rcl_interfaces::msg::SetParametersResult result; + result.successful = successful; + return result; + }; } -TEST_F(TestNode, test_registering_multiple_callbacks_api) { +TEST_F(TestNode, test_registering_multiple_callbacks_api) +{ auto node = std::make_shared("test_declare_parameter_node"_unq); { int64_t default_value{42}; auto name1 = "parameter"_unq; - auto scoped_callback1 = node->add_on_set_parameters_callback( - get_fixed_on_parameter_set_callback(name1, true)); + auto scoped_callback1 = + node->add_on_set_parameters_callback(get_fixed_on_parameter_set_callback(name1, true)); EXPECT_NE(scoped_callback1, nullptr); int64_t value{node->declare_parameter(name1, default_value)}; EXPECT_EQ(value, default_value); auto name2 = "parameter"_unq; - auto scoped_callback2 = node->add_on_set_parameters_callback( - get_fixed_on_parameter_set_callback(name2, false)); + auto scoped_callback2 = + node->add_on_set_parameters_callback(get_fixed_on_parameter_set_callback(name2, false)); EXPECT_NE(scoped_callback2, nullptr); EXPECT_THROW( - {node->declare_parameter(name2, default_value);}, + { node->declare_parameter(name2, default_value); }, rclcpp::exceptions::InvalidParameterValueException); auto name3 = "parameter"_unq; @@ -414,18 +410,18 @@ TEST_F(TestNode, test_registering_multiple_callbacks_api) { { int64_t default_value{42}; auto name1 = "parameter"_unq; - auto scoped_callback1 = node->add_on_set_parameters_callback( - get_fixed_on_parameter_set_callback(name1, true)); + auto scoped_callback1 = + node->add_on_set_parameters_callback(get_fixed_on_parameter_set_callback(name1, true)); EXPECT_NE(scoped_callback1, nullptr); int64_t value{node->declare_parameter(name1, default_value)}; EXPECT_EQ(value, default_value); auto name2 = "parameter"_unq; - auto scoped_callback2 = node->add_on_set_parameters_callback( - get_fixed_on_parameter_set_callback(name2, false)); + auto scoped_callback2 = + node->add_on_set_parameters_callback(get_fixed_on_parameter_set_callback(name2, false)); EXPECT_NE(scoped_callback2, nullptr); EXPECT_THROW( - {node->declare_parameter(name2, default_value);}, + { node->declare_parameter(name2, default_value); }, rclcpp::exceptions::InvalidParameterValueException); auto name3 = "parameter"_unq; @@ -437,13 +433,12 @@ TEST_F(TestNode, test_registering_multiple_callbacks_api) { int64_t default_value{42}; auto name1 = "parameter"_unq; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr scoped_callback( - node->add_on_set_parameters_callback( - get_fixed_on_parameter_set_callback(name1, false))); + node->add_on_set_parameters_callback(get_fixed_on_parameter_set_callback(name1, false))); rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr scoped_callback_copy(scoped_callback); scoped_callback.reset(); EXPECT_THROW( - {node->declare_parameter("parameter"_unq, default_value);}, + { node->declare_parameter("parameter"_unq, default_value); }, rclcpp::exceptions::InvalidParameterValueException); scoped_callback_copy.reset(); @@ -453,11 +448,11 @@ TEST_F(TestNode, test_registering_multiple_callbacks_api) { } } -TEST_F(TestNode, declare_parameter_with_overrides) { +TEST_F(TestNode, declare_parameter_with_overrides) +{ // test cases with overrides rclcpp::NodeOptions no; - no.parameter_overrides( - { + no.parameter_overrides({ {"parameter_no_default", 42}, {"parameter_no_default_set", 42}, {"parameter_no_default_set_cvref", 42}, @@ -537,61 +532,55 @@ TEST_F(TestNode, declare_parameter_with_overrides) { auto name = "parameter_already_declared"; node->declare_parameter(name); EXPECT_THROW( - {node->declare_parameter(name);}, - rclcpp::exceptions::ParameterAlreadyDeclaredException); + { node->declare_parameter(name); }, rclcpp::exceptions::ParameterAlreadyDeclaredException); } { // parameter name invalid throws - EXPECT_THROW( - {node->declare_parameter("");}, - rclcpp::exceptions::InvalidParametersException); + EXPECT_THROW({ node->declare_parameter(""); }, rclcpp::exceptions::InvalidParametersException); } { // parameter rejected throws, with initial value - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset auto name = std::string("parameter_rejected"); - auto on_set_parameters = - [&name](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - for (const auto & parameter : parameters) { - if ( - parameter.get_name() == name && - parameter.get_type() == rclcpp::PARAMETER_INTEGER) - { - if (parameter.get_value() < 43) { - result.successful = false; - result.reason = "'" + name + "' must be an integer and less than 43"; - } + auto on_set_parameters = [&name](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if (parameter.get_name() == name && parameter.get_type() == rclcpp::PARAMETER_INTEGER) { + if (parameter.get_value() < 43) { + result.successful = false; + result.reason = "'" + name + "' must be an integer and less than 43"; } } - return result; - }; + } + return result; + }; EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); EXPECT_THROW( - {node->declare_parameter(name, 43);}, + { node->declare_parameter(name, 43); }, rclcpp::exceptions::InvalidParameterValueException); } { // default type and initial value type do not match EXPECT_THROW( - {node->declare_parameter("parameter_type_mismatch", 42);}, - rclcpp::ParameterTypeException); + { node->declare_parameter("parameter_type_mismatch", 42); }, rclcpp::ParameterTypeException); } } -TEST_F(TestNode, declare_parameters_with_no_initial_values) { +TEST_F(TestNode, declare_parameters_with_no_initial_values) +{ // test cases without initial values auto node = std::make_shared("test_declare_parameters_node"_unq); { // with namespace, defaults, no custom descriptors, no initial int64_t bigger_than_int = INT64_MAX - 42; auto values = node->declare_parameters( - "namespace1", { - {"parameter_a", 42}, - {"parameter_b", 256}, - {"parameter_c", bigger_than_int}, - }); + "namespace1", + { + {"parameter_a", 42}, + {"parameter_b", 256}, + {"parameter_c", bigger_than_int}, + }); std::vector expected = {42, 256, bigger_than_int}; EXPECT_EQ(values, expected); EXPECT_TRUE(node->has_parameter("namespace1.parameter_a")); @@ -601,10 +590,11 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { { // without namespace, defaults, no custom descriptors, no initial auto values = node->declare_parameters( - "", { - {"parameter_without_ns_a", 42}, - {"parameter_without_ns_b", 256}, - }); + "", + { + {"parameter_without_ns_a", 42}, + {"parameter_without_ns_b", 256}, + }); std::vector expected = {42, 256}; EXPECT_EQ(values, expected); EXPECT_TRUE(node->has_parameter("parameter_without_ns_a")); @@ -615,10 +605,11 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.read_only = true; auto values = node->declare_parameters( - "namespace2", { - {"parameter_a", {42, descriptor}}, - {"parameter_b", {256, descriptor}}, - }); + "namespace2", + { + {"parameter_a", {42, descriptor}}, + {"parameter_b", {256, descriptor}}, + }); std::vector expected = {42, 256}; EXPECT_EQ(values, expected); EXPECT_TRUE(node->has_parameter("namespace2.parameter_a")); @@ -630,10 +621,11 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.read_only = true; auto values = node->declare_parameters( - "", { - {"parameter_without_ns_c", {42, descriptor}}, - {"parameter_without_ns_d", {256, descriptor}}, - }); + "", + { + {"parameter_without_ns_c", {42, descriptor}}, + {"parameter_without_ns_d", {256, descriptor}}, + }); std::vector expected = {42, 256}; EXPECT_EQ(values, expected); EXPECT_TRUE(node->has_parameter("parameter_without_ns_c")); @@ -642,7 +634,7 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { { // empty parameters auto values = node->declare_parameters("", {}); - std::vector expected {}; + std::vector expected{}; EXPECT_EQ(values, expected); } { @@ -650,42 +642,45 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { auto name = "parameter"_unq; node->declare_parameter(name); EXPECT_THROW( - {node->declare_parameters("", {{name, 42}});}, + { + node->declare_parameters("", {{name, 42}}); + }, rclcpp::exceptions::ParameterAlreadyDeclaredException); } { // parameter name invalid throws EXPECT_THROW( - {node->declare_parameters("", {{"", 42}});}, + { + node->declare_parameters("", {{"", 42}}); + }, rclcpp::exceptions::InvalidParametersException); } { // parameter rejected throws - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset auto name = "parameter"_unq; - auto on_set_parameters = - [&name](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - for (const auto & parameter : parameters) { - if ( - parameter.get_name() == name && - parameter.get_type() != rclcpp::PARAMETER_INTEGER) - { - result.successful = false; - result.reason = "'" + name + "' must be an integer"; - } + auto on_set_parameters = [&name](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if (parameter.get_name() == name && parameter.get_type() != rclcpp::PARAMETER_INTEGER) { + result.successful = false; + result.reason = "'" + name + "' must be an integer"; } - return result; - }; + } + return result; + }; EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); EXPECT_THROW( - {node->declare_parameters("", {{name, "not an int"}});}, + { + node->declare_parameters("", {{name, "not an int"}}); + }, rclcpp::exceptions::InvalidParameterValueException); } } -TEST_F(TestNode, undeclare_parameter) { +TEST_F(TestNode, undeclare_parameter) +{ auto node = std::make_shared("test_undeclare_parameter_node"_unq); { // normal use @@ -700,8 +695,7 @@ TEST_F(TestNode, undeclare_parameter) { auto name = "parameter"_unq; EXPECT_FALSE(node->has_parameter(name)); EXPECT_THROW( - {node->undeclare_parameter(name);}, - rclcpp::exceptions::ParameterNotDeclaredException); + { node->undeclare_parameter(name); }, rclcpp::exceptions::ParameterNotDeclaredException); } { // read only parameter throws @@ -711,13 +705,13 @@ TEST_F(TestNode, undeclare_parameter) { node->declare_parameter(name, 42, descriptor); EXPECT_TRUE(node->has_parameter(name)); EXPECT_THROW( - {node->undeclare_parameter(name);}, - rclcpp::exceptions::ParameterImmutableException); + { node->undeclare_parameter(name); }, rclcpp::exceptions::ParameterImmutableException); EXPECT_TRUE(node->has_parameter(name)); } } -TEST_F(TestNode, has_parameter) { +TEST_F(TestNode, has_parameter) +{ auto node = std::make_shared("test_has_parameter_node"_unq); { // normal use @@ -730,10 +724,10 @@ TEST_F(TestNode, has_parameter) { } } -TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { +TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) +{ auto node = std::make_shared( - "test_set_parameter_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(false)); + "test_set_parameter_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(false)); { // normal use auto name = "parameter"_unq; @@ -779,7 +773,7 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { auto name = "parameter"_unq; EXPECT_FALSE(node->has_parameter(name)); EXPECT_THROW( - {node->set_parameter(rclcpp::Parameter(name, 42));}, + { node->set_parameter(rclcpp::Parameter(name, 42)); }, rclcpp::exceptions::ParameterNotDeclaredException); EXPECT_FALSE(node->has_parameter(name)); } @@ -788,14 +782,13 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { auto name = "parameter"_unq; node->declare_parameter(name, 42); - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset - auto on_set_parameters = - [](const std::vector &) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = false; - result.reason = "no parameter may not be set right now"; - return result; - }; + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset + auto on_set_parameters = [](const std::vector &) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = "no parameter may not be set right now"; + return result; + }; node->set_on_parameters_set_callback(on_set_parameters); EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); @@ -1173,10 +1166,10 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { } } -TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) { +TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) +{ rclcpp::NodeOptions no; - no.parameter_overrides( - { + no.parameter_overrides({ {"parameter_with_override", 30}, }); no.allow_undeclared_parameters(true); @@ -1210,10 +1203,10 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) { } } -TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { +TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) +{ auto node = std::make_shared( - "test_set_parameters_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(false)); + "test_set_parameters_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(false)); { // normal use auto name1 = "parameter"_unq; @@ -1223,13 +1216,12 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { node->declare_parameter(name2, true); node->declare_parameter(name3, "blue"); - auto rets = node->set_parameters( - { + auto rets = node->set_parameters({ {name1, 2}, {name2, false}, {name3, "red"}, }); - EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) { return r.successful; })); EXPECT_TRUE(node->has_parameter(name1)); EXPECT_TRUE(node->has_parameter(name2)); EXPECT_TRUE(node->has_parameter(name3)); @@ -1239,12 +1231,11 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { auto name = "parameter"_unq; node->declare_parameter(name, 1); - auto rets = node->set_parameters( - { + auto rets = node->set_parameters({ {name, 42}, {name, 2}, }); - EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) { return r.successful; })); EXPECT_TRUE(node->has_parameter(name)); EXPECT_EQ(node->get_parameter(name).get_value(), 2); } @@ -1258,14 +1249,13 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { node->declare_parameter(name3, 100); EXPECT_THROW( - { - node->set_parameters( { - {name1, 2}, - {name2, "not declared :("}, - {name3, 101}, - }); - }, + node->set_parameters({ + {name1, 2}, + {name2, "not declared :("}, + {name3, 101}, + }); + }, rclcpp::exceptions::ParameterNotDeclaredException); EXPECT_TRUE(node->has_parameter(name1)); @@ -1284,21 +1274,19 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { node->declare_parameter(name2, true); node->declare_parameter(name3, "blue"); - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset - auto on_set_parameters = - [&name2](const std::vector & ps) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - if (std::any_of(ps.begin(), ps.end(), [&](auto & p) {return p.get_name() == name2;})) { - result.successful = false; - result.reason = "parameter '" + name2 + "' may not be set right now"; - } - return result; - }; + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset + auto on_set_parameters = [&name2](const std::vector & ps) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (std::any_of(ps.begin(), ps.end(), [&](auto & p) { return p.get_name() == name2; })) { + result.successful = false; + result.reason = "parameter '" + name2 + "' may not be set right now"; + } + return result; + }; node->set_on_parameters_set_callback(on_set_parameters); - auto rets = node->set_parameters( - { + auto rets = node->set_parameters({ {name1, 2}, {name2, false}, {name3, "red"}, @@ -1339,10 +1327,10 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { } // test set_parameters with undeclared allowed -TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) { +TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) +{ auto node = std::make_shared( - "test_set_parameters_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(true)); + "test_set_parameters_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(true)); { // normal use (declare first) still works with this true auto name1 = "parameter"_unq; @@ -1358,12 +1346,11 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) { EXPECT_TRUE(node->has_parameter(name2)); EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); - auto rets = node->set_parameters( - { + auto rets = node->set_parameters({ rclcpp::Parameter(name1, 43), rclcpp::Parameter(name2, "other"), }); - EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) { return r.successful; })); EXPECT_EQ(node->get_parameter(name1).get_value(), 43); EXPECT_EQ(node->get_parameter(name2).get_value(), "other"); } @@ -1375,18 +1362,18 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) { EXPECT_FALSE(node->has_parameter(name1)); EXPECT_FALSE(node->has_parameter(name2)); - auto rets = node->set_parameters( - { + auto rets = node->set_parameters({ rclcpp::Parameter(name1, 42), rclcpp::Parameter(name2, "test"), }); - EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) { return r.successful; })); EXPECT_EQ(node->get_parameter(name1).get_value(), 42); EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); } } -TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { +TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) +{ auto node = std::make_shared( "test_set_parameters_atomically_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(false)); @@ -1399,8 +1386,7 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { node->declare_parameter(name2, true); node->declare_parameter(name3, "blue"); - auto ret = node->set_parameters_atomically( - { + auto ret = node->set_parameters_atomically({ {name1, 2}, {name2, false}, {name3, "red"}, @@ -1415,8 +1401,7 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { auto name = "parameter"_unq; node->declare_parameter(name, 1); - auto ret = node->set_parameters_atomically( - { + auto ret = node->set_parameters_atomically({ {name, 42}, {name, 2}, }); @@ -1434,14 +1419,13 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { node->declare_parameter(name3, 100); EXPECT_THROW( - { - node->set_parameters_atomically( { - {name1, 2}, - {name2, "not declared :("}, - {name3, 101}, - }); - }, + node->set_parameters_atomically({ + {name1, 2}, + {name2, "not declared :("}, + {name3, 101}, + }); + }, rclcpp::exceptions::ParameterNotDeclaredException); EXPECT_TRUE(node->has_parameter(name1)); @@ -1461,21 +1445,19 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { node->declare_parameter(name2, true); node->declare_parameter(name3, "blue"); - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset - auto on_set_parameters = - [&name2](const std::vector & ps) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - if (std::any_of(ps.begin(), ps.end(), [&](auto & p) {return p.get_name() == name2;})) { - result.successful = false; - result.reason = "parameter '" + name2 + "' may not be set right now"; - } - return result; - }; + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset + auto on_set_parameters = [&name2](const std::vector & ps) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (std::any_of(ps.begin(), ps.end(), [&](auto & p) { return p.get_name() == name2; })) { + result.successful = false; + result.reason = "parameter '" + name2 + "' may not be set right now"; + } + return result; + }; node->set_on_parameters_set_callback(on_set_parameters); - auto ret = node->set_parameters_atomically( - { + auto ret = node->set_parameters_atomically({ {name1, 2}, {name2, false}, // should fail to be set, failing the whole operation {name3, "red"}, @@ -1513,7 +1495,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { } // test set_parameters with undeclared allowed -TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { +TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) +{ auto node = std::make_shared( "test_set_parameters_atomically_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(true)); @@ -1532,8 +1515,7 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { EXPECT_TRUE(node->has_parameter(name2)); EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); - auto ret = node->set_parameters_atomically( - { + auto ret = node->set_parameters_atomically({ rclcpp::Parameter(name1, 43), rclcpp::Parameter(name2, "other"), }); @@ -1549,8 +1531,7 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { EXPECT_FALSE(node->has_parameter(name1)); EXPECT_FALSE(node->has_parameter(name2)); - auto ret = node->set_parameters_atomically( - { + auto ret = node->set_parameters_atomically({ rclcpp::Parameter(name1, 42), rclcpp::Parameter(name2, "test"), }); @@ -1572,23 +1553,21 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { EXPECT_TRUE(node->has_parameter(name3)); EXPECT_EQ(node->get_parameter(name3).get_value(), "test"); - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset - auto on_set_parameters = - [&name3](const std::vector & ps) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - if (std::any_of(ps.begin(), ps.end(), [&](auto & p) {return p.get_name() == name3;})) { - result.successful = false; - result.reason = "parameter '" + name3 + "' may not be set right now"; - } - return result; - }; + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset + auto on_set_parameters = [&name3](const std::vector & ps) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (std::any_of(ps.begin(), ps.end(), [&](auto & p) { return p.get_name() == name3; })) { + result.successful = false; + result.reason = "parameter '" + name3 + "' may not be set right now"; + } + return result; + }; node->set_on_parameters_set_callback(on_set_parameters); - auto ret = node->set_parameters_atomically( - { + auto ret = node->set_parameters_atomically({ rclcpp::Parameter(name1, 43), - rclcpp::Parameter(name2, true), // this would cause implicit declaration + rclcpp::Parameter(name2, true), // this would cause implicit declaration rclcpp::Parameter(name3, "other"), // this set should fail, and fail the whole operation }); EXPECT_FALSE(ret.successful); @@ -1600,10 +1579,10 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { } // test get_parameter with undeclared not allowed -TEST_F(TestNode, get_parameter_undeclared_parameters_not_allowed) { +TEST_F(TestNode, get_parameter_undeclared_parameters_not_allowed) +{ auto node = std::make_shared( - "test_get_parameter_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(false)); + "test_get_parameter_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(false)); { // normal use auto name = "parameter"_unq; @@ -1631,7 +1610,7 @@ TEST_F(TestNode, get_parameter_undeclared_parameters_not_allowed) { auto name = "parameter"_unq; EXPECT_FALSE(node->has_parameter(name)); - EXPECT_THROW({node->get_parameter(name);}, rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_THROW({ node->get_parameter(name); }, rclcpp::exceptions::ParameterNotDeclaredException); { rclcpp::Parameter parameter; EXPECT_FALSE(node->get_parameter(name, parameter)); @@ -1648,19 +1627,19 @@ TEST_F(TestNode, get_parameter_undeclared_parameters_not_allowed) { node->declare_parameter(name, "not an int"); EXPECT_THROW( - { - int value; - node->get_parameter(name, value); - }, + { + int value; + node->get_parameter(name, value); + }, rclcpp::ParameterTypeException); } } // test get_parameter with undeclared allowed -TEST_F(TestNode, get_parameter_undeclared_parameters_allowed) { +TEST_F(TestNode, get_parameter_undeclared_parameters_allowed) +{ auto node = std::make_shared( - "test_get_parameter_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(true)); + "test_get_parameter_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(true)); { // normal use (declare first) still works auto name = "parameter"_unq; @@ -1710,10 +1689,10 @@ TEST_F(TestNode, get_parameter_undeclared_parameters_allowed) { } // test get_parameter_or with undeclared not allowed -TEST_F(TestNode, get_parameter_or_undeclared_parameters_not_allowed) { +TEST_F(TestNode, get_parameter_or_undeclared_parameters_not_allowed) +{ auto node = std::make_shared( - "test_get_parameter_or_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(false)); + "test_get_parameter_or_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(false)); { // normal use (declare first) still works auto name = "parameter"_unq; @@ -1740,10 +1719,10 @@ TEST_F(TestNode, get_parameter_or_undeclared_parameters_not_allowed) { } // test get_parameter_or with undeclared allowed -TEST_F(TestNode, get_parameter_or_undeclared_parameters_allowed) { +TEST_F(TestNode, get_parameter_or_undeclared_parameters_allowed) +{ auto node = std::make_shared( - "test_get_parameter_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(true)); + "test_get_parameter_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(true)); { // normal use (declare first) still works auto name = "parameter"_unq; @@ -1770,10 +1749,10 @@ TEST_F(TestNode, get_parameter_or_undeclared_parameters_allowed) { } // test get_parameters with undeclared not allowed -TEST_F(TestNode, get_parameters_undeclared_parameters_not_allowed) { +TEST_F(TestNode, get_parameters_undeclared_parameters_not_allowed) +{ auto node = std::make_shared( - "test_get_parameters_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(false)); + "test_get_parameters_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(false)); { // normal use auto base_name1 = "parameter"_unq; @@ -1849,8 +1828,7 @@ TEST_F(TestNode, get_parameters_undeclared_parameters_not_allowed) { EXPECT_FALSE(node->has_parameter(name)); EXPECT_THROW( - {node->get_parameters({name});}, - rclcpp::exceptions::ParameterNotDeclaredException); + { node->get_parameters({name}); }, rclcpp::exceptions::ParameterNotDeclaredException); { std::map values; EXPECT_TRUE(values.empty()); @@ -1882,20 +1860,16 @@ TEST_F(TestNode, get_parameters_undeclared_parameters_not_allowed) { { std::map actual; - EXPECT_THROW( - { - node_local->get_parameters("", actual); - }, - rclcpp::ParameterTypeException); + EXPECT_THROW({ node_local->get_parameters("", actual); }, rclcpp::ParameterTypeException); } } } // test get_parameters with undeclared allowed -TEST_F(TestNode, get_parameters_undeclared_parameters_allowed) { +TEST_F(TestNode, get_parameters_undeclared_parameters_allowed) +{ auto node = std::make_shared( - "test_get_parameters_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(true)); + "test_get_parameters_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(true)); { // normal use auto base_name1 = "parameter"_unq; @@ -1934,10 +1908,10 @@ TEST_F(TestNode, get_parameters_undeclared_parameters_allowed) { } // test describe parameter with undeclared not allowed -TEST_F(TestNode, describe_parameter_undeclared_parameters_not_allowed) { +TEST_F(TestNode, describe_parameter_undeclared_parameters_not_allowed) +{ auto node = std::make_shared( - "test_get_parameter_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(false)); + "test_get_parameter_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(false)); { // normal use auto name1 = "parameter"_unq; @@ -1967,18 +1941,16 @@ TEST_F(TestNode, describe_parameter_undeclared_parameters_not_allowed) { { EXPECT_THROW( - { - node->describe_parameter(name); - }, rclcpp::exceptions::ParameterNotDeclaredException); + { node->describe_parameter(name); }, rclcpp::exceptions::ParameterNotDeclaredException); } } } // test describe parameter with undeclared allowed -TEST_F(TestNode, describe_parameter_undeclared_parameters_allowed) { +TEST_F(TestNode, describe_parameter_undeclared_parameters_allowed) +{ auto node = std::make_shared( - "test_get_parameter_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(true)); + "test_get_parameter_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(true)); { // normal use still works auto name1 = "parameter"_unq; @@ -2016,10 +1988,10 @@ TEST_F(TestNode, describe_parameter_undeclared_parameters_allowed) { } // test describe parameters with undeclared not allowed -TEST_F(TestNode, describe_parameters_undeclared_parameters_not_allowed) { +TEST_F(TestNode, describe_parameters_undeclared_parameters_not_allowed) +{ auto node = std::make_shared( - "test_get_parameters_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(false)); + "test_get_parameters_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(false)); { // normal use auto name1 = "parameter"_unq; @@ -2048,9 +2020,7 @@ TEST_F(TestNode, describe_parameters_undeclared_parameters_not_allowed) { { EXPECT_THROW( - { - node->describe_parameters({name}); - }, rclcpp::exceptions::ParameterNotDeclaredException); + { node->describe_parameters({name}); }, rclcpp::exceptions::ParameterNotDeclaredException); } } { @@ -2062,9 +2032,10 @@ TEST_F(TestNode, describe_parameters_undeclared_parameters_not_allowed) { { EXPECT_THROW( - { - node->describe_parameters({name1, name2}); - }, rclcpp::exceptions::ParameterNotDeclaredException); + { + node->describe_parameters({name1, name2}); + }, + rclcpp::exceptions::ParameterNotDeclaredException); } } { @@ -2096,10 +2067,10 @@ TEST_F(TestNode, describe_parameters_undeclared_parameters_not_allowed) { } // test describe parameters with undeclared allowed -TEST_F(TestNode, describe_parameters_undeclared_parameters_allowed) { +TEST_F(TestNode, describe_parameters_undeclared_parameters_allowed) +{ auto node = std::make_shared( - "test_get_parameters_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(true)); + "test_get_parameters_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(true)); { // normal use still works (declare first) auto name1 = "parameter"_unq; @@ -2158,10 +2129,10 @@ TEST_F(TestNode, describe_parameters_undeclared_parameters_allowed) { } // test get parameter types with undeclared not allowed -TEST_F(TestNode, get_parameter_types_undeclared_parameters_not_allowed) { +TEST_F(TestNode, get_parameter_types_undeclared_parameters_not_allowed) +{ auto node = std::make_shared( - "test_get_parameter_types_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(false)); + "test_get_parameter_types_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(false)); { // normal use auto name1 = "parameter"_unq; @@ -2186,9 +2157,7 @@ TEST_F(TestNode, get_parameter_types_undeclared_parameters_not_allowed) { { EXPECT_THROW( - { - node->get_parameter_types({name}); - }, rclcpp::exceptions::ParameterNotDeclaredException); + { node->get_parameter_types({name}); }, rclcpp::exceptions::ParameterNotDeclaredException); } } { @@ -2214,10 +2183,10 @@ TEST_F(TestNode, get_parameter_types_undeclared_parameters_not_allowed) { } // test get parameter types with undeclared allowed -TEST_F(TestNode, get_parameter_types_undeclared_parameters_allowed) { +TEST_F(TestNode, get_parameter_types_undeclared_parameters_allowed) +{ auto node = std::make_shared( - "test_get_parameter_types_node"_unq, - rclcpp::NodeOptions().allow_undeclared_parameters(true)); + "test_get_parameter_types_node"_unq, rclcpp::NodeOptions().allow_undeclared_parameters(true)); { // normal use still works (declare first) auto name1 = "parameter"_unq; @@ -2264,7 +2233,8 @@ TEST_F(TestNode, get_parameter_types_undeclared_parameters_allowed) { } // test that it is possible to call get_parameter within the set_callback -TEST_F(TestNode, set_on_parameters_set_callback_get_parameter) { +TEST_F(TestNode, set_on_parameters_set_callback_get_parameter) +{ auto node = std::make_shared("test_set_callback_get_parameter_node"_unq); int64_t intval = node->declare_parameter("intparam", 42); @@ -2273,27 +2243,26 @@ TEST_F(TestNode, set_on_parameters_set_callback_get_parameter) { EXPECT_EQ(floatval, 5.4); double floatout; - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset - auto on_set_parameters = - [&node, &floatout](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - if (parameters.size() != 1) { - result.successful = false; - } + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset + auto on_set_parameters = [&node, &floatout](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } - if (parameters[0].get_value() != 40) { - result.successful = false; - } + if (parameters[0].get_value() != 40) { + result.successful = false; + } - rclcpp::Parameter floatparam = node->get_parameter("floatparam"); - if (floatparam.get_value() != 5.4) { - result.successful = false; - } - floatout = floatparam.get_value(); + rclcpp::Parameter floatparam = node->get_parameter("floatparam"); + if (floatparam.get_value() != 5.4) { + result.successful = false; + } + floatout = floatparam.get_value(); - return result; - }; + return result; + }; EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); ASSERT_NO_THROW(node->set_parameter({"intparam", 40})); @@ -2301,7 +2270,8 @@ TEST_F(TestNode, set_on_parameters_set_callback_get_parameter) { } // test that calling set_parameter inside of a set_callback throws an exception -TEST_F(TestNode, set_on_parameters_set_callback_set_parameter) { +TEST_F(TestNode, set_on_parameters_set_callback_set_parameter) +{ auto node = std::make_shared("test_set_callback_set_parameter_node"_unq); int64_t intval = node->declare_parameter("intparam", 42); @@ -2309,34 +2279,33 @@ TEST_F(TestNode, set_on_parameters_set_callback_set_parameter) { double floatval = node->declare_parameter("floatparam", 5.4); EXPECT_EQ(floatval, 5.4); - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset - auto on_set_parameters = - [&node](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - if (parameters.size() != 1) { - result.successful = false; - } + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset + auto on_set_parameters = [&node](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } - if (parameters[0].get_value() != 40) { - result.successful = false; - } + if (parameters[0].get_value() != 40) { + result.successful = false; + } - // This should throw an exception - node->set_parameter({"floatparam", 5.6}); + // This should throw an exception + node->set_parameter({"floatparam", 5.6}); - return result; - }; + return result; + }; EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); EXPECT_THROW( - { - node->set_parameter(rclcpp::Parameter("intparam", 40)); - }, rclcpp::exceptions::ParameterModifiedInCallbackException); + { node->set_parameter(rclcpp::Parameter("intparam", 40)); }, + rclcpp::exceptions::ParameterModifiedInCallbackException); } // test that calling declare_parameter inside of a set_callback throws an exception -TEST_F(TestNode, set_on_parameters_set_callback_declare_parameter) { +TEST_F(TestNode, set_on_parameters_set_callback_declare_parameter) +{ auto node = std::make_shared("test_set_callback_declare_parameter_node"_unq); int64_t intval = node->declare_parameter("intparam", 42); @@ -2344,34 +2313,33 @@ TEST_F(TestNode, set_on_parameters_set_callback_declare_parameter) { double floatval = node->declare_parameter("floatparam", 5.4); EXPECT_EQ(floatval, 5.4); - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset - auto on_set_parameters = - [&node](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - if (parameters.size() != 1) { - result.successful = false; - } + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset + auto on_set_parameters = [&node](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } - if (parameters[0].get_value() != 40) { - result.successful = false; - } + if (parameters[0].get_value() != 40) { + result.successful = false; + } - // This should throw an exception - node->declare_parameter("floatparam2", 5.6); + // This should throw an exception + node->declare_parameter("floatparam2", 5.6); - return result; - }; + return result; + }; EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); EXPECT_THROW( - { - node->set_parameter(rclcpp::Parameter("intparam", 40)); - }, rclcpp::exceptions::ParameterModifiedInCallbackException); + { node->set_parameter(rclcpp::Parameter("intparam", 40)); }, + rclcpp::exceptions::ParameterModifiedInCallbackException); } // test that calling undeclare_parameter inside a set_callback throws an exception -TEST_F(TestNode, set_on_parameters_set_callback_undeclare_parameter) { +TEST_F(TestNode, set_on_parameters_set_callback_undeclare_parameter) +{ auto node = std::make_shared("test_set_callback_undeclare_parameter_node"_unq); int64_t intval = node->declare_parameter("intparam", 42); @@ -2379,34 +2347,33 @@ TEST_F(TestNode, set_on_parameters_set_callback_undeclare_parameter) { double floatval = node->declare_parameter("floatparam", 5.4); EXPECT_EQ(floatval, 5.4); - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset - auto on_set_parameters = - [&node](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - if (parameters.size() != 1) { - result.successful = false; - } + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset + auto on_set_parameters = [&node](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } - if (parameters[0].get_value() != 40) { - result.successful = false; - } + if (parameters[0].get_value() != 40) { + result.successful = false; + } - // This should throw an exception - node->undeclare_parameter("floatparam"); + // This should throw an exception + node->undeclare_parameter("floatparam"); - return result; - }; + return result; + }; EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); EXPECT_THROW( - { - node->set_parameter(rclcpp::Parameter("intparam", 40)); - }, rclcpp::exceptions::ParameterModifiedInCallbackException); + { node->set_parameter(rclcpp::Parameter("intparam", 40)); }, + rclcpp::exceptions::ParameterModifiedInCallbackException); } // test that calling set_on_parameters_set_callback from a set_callback throws an exception -TEST_F(TestNode, set_on_parameters_set_callback_set_on_parameters_set_callback) { +TEST_F(TestNode, set_on_parameters_set_callback_set_on_parameters_set_callback) +{ auto node = std::make_shared("test_set_callback_set_callback_node"_unq); int64_t intval = node->declare_parameter("intparam", 42); @@ -2414,35 +2381,32 @@ TEST_F(TestNode, set_on_parameters_set_callback_set_on_parameters_set_callback) double floatval = node->declare_parameter("floatparam", 5.4); EXPECT_EQ(floatval, 5.4); - RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset - auto on_set_parameters = - [&node](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - if (parameters.size() != 1) { - result.successful = false; - } - - if (parameters[0].get_value() != 40) { - result.successful = false; - } - - auto bad_parameters = - [](const std::vector & parameters) { - (void)parameters; - rcl_interfaces::msg::SetParametersResult result; - return result; - }; + RCLCPP_SCOPE_EXIT({ node->set_on_parameters_set_callback(nullptr); }); // always reset + auto on_set_parameters = [&node](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } - // This should throw an exception - node->set_on_parameters_set_callback(bad_parameters); + if (parameters[0].get_value() != 40) { + result.successful = false; + } + auto bad_parameters = [](const std::vector & parameters) { + (void)parameters; + rcl_interfaces::msg::SetParametersResult result; return result; }; + // This should throw an exception + node->set_on_parameters_set_callback(bad_parameters); + + return result; + }; + EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); EXPECT_THROW( - { - node->set_parameter(rclcpp::Parameter("intparam", 40)); - }, rclcpp::exceptions::ParameterModifiedInCallbackException); + { node->set_parameter(rclcpp::Parameter("intparam", 40)); }, + rclcpp::exceptions::ParameterModifiedInCallbackException); } diff --git a/rclcpp/test/test_node_global_args.cpp b/rclcpp/test/test_node_global_args.cpp index febef6bb37..9536e8b61d 100644 --- a/rclcpp/test/test_node_global_args.cpp +++ b/rclcpp/test/test_node_global_args.cpp @@ -14,8 +14,8 @@ #include -#include #include +#include #include #include "rclcpp/exceptions.hpp" @@ -34,25 +34,25 @@ class TestNodeWithGlobalArgs : public ::testing::Test } }; -TEST_F(TestNodeWithGlobalArgs, local_arguments_before_global) { - auto options = rclcpp::NodeOptions() - .arguments({"--ros-args", "-r", "__node:=local_arguments_test"}); +TEST_F(TestNodeWithGlobalArgs, local_arguments_before_global) +{ + auto options = + rclcpp::NodeOptions().arguments({"--ros-args", "-r", "__node:=local_arguments_test"}); auto node = rclcpp::Node::make_shared("orig_name", options); EXPECT_STREQ("local_arguments_test", node->get_name()); } -TEST_F(TestNodeWithGlobalArgs, use_or_ignore_global_arguments) { +TEST_F(TestNodeWithGlobalArgs, use_or_ignore_global_arguments) +{ { // Don't use global args - auto options = rclcpp::NodeOptions() - .use_global_arguments(false); + auto options = rclcpp::NodeOptions().use_global_arguments(false); auto node = rclcpp::Node::make_shared("orig_name", options); EXPECT_STREQ("orig_name", node->get_name()); } { // Do use global args - auto options = rclcpp::NodeOptions() - .use_global_arguments(true); + auto options = rclcpp::NodeOptions().use_global_arguments(true); auto node = rclcpp::Node::make_shared("orig_name", options); EXPECT_STREQ("global_node_name", node->get_name()); diff --git a/rclcpp/test/test_node_options.cpp b/rclcpp/test/test_node_options.cpp index c4eb969e85..6381727e1f 100644 --- a/rclcpp/test/test_node_options.cpp +++ b/rclcpp/test/test_node_options.cpp @@ -23,11 +23,11 @@ #include "rclcpp/node_options.hpp" - -TEST(TestNodeOptions, ros_args_only) { +TEST(TestNodeOptions, ros_args_only) +{ rcl_allocator_t allocator = rcl_get_default_allocator(); - auto options = rclcpp::NodeOptions(allocator) - .arguments({"--ros-args", "-r", "__node:=some_node", "-r", "__ns:=/some_ns"}); + auto options = rclcpp::NodeOptions(allocator).arguments( + {"--ros-args", "-r", "__node:=some_node", "-r", "__ns:=/some_ns"}); const rcl_node_options_t * rcl_options = options.get_rcl_node_options(); ASSERT_TRUE(rcl_options != nullptr); @@ -35,25 +35,34 @@ TEST(TestNodeOptions, ros_args_only) { ASSERT_EQ(0, rcl_arguments_get_count_unparsed_ros(&rcl_options->arguments)); char * node_name = nullptr; - EXPECT_EQ(RCL_RET_OK, rcl_remap_node_name( - &rcl_options->arguments, nullptr, "my_node", allocator, &node_name)); + EXPECT_EQ( + RCL_RET_OK, + rcl_remap_node_name(&rcl_options->arguments, nullptr, "my_node", allocator, &node_name)); ASSERT_TRUE(node_name != nullptr); EXPECT_STREQ("some_node", node_name); allocator.deallocate(node_name, allocator.state); char * namespace_name = nullptr; - EXPECT_EQ(RCL_RET_OK, rcl_remap_node_namespace( + EXPECT_EQ( + RCL_RET_OK, + rcl_remap_node_namespace( &rcl_options->arguments, nullptr, "my_ns", allocator, &namespace_name)); ASSERT_TRUE(namespace_name != nullptr); EXPECT_STREQ("/some_ns", namespace_name); allocator.deallocate(namespace_name, allocator.state); } -TEST(TestNodeOptions, ros_args_and_non_ros_args) { +TEST(TestNodeOptions, ros_args_and_non_ros_args) +{ rcl_allocator_t allocator = rcl_get_default_allocator(); - auto options = rclcpp::NodeOptions(allocator).arguments({ - "--non-ros-flag", "--ros-args", "-r", "__node:=some_node", - "-r", "__ns:=/some_ns", "--", "non-ros-arg"}); + auto options = rclcpp::NodeOptions(allocator).arguments({"--non-ros-flag", + "--ros-args", + "-r", + "__node:=some_node", + "-r", + "__ns:=/some_ns", + "--", + "non-ros-arg"}); const rcl_node_options_t * rcl_options = options.get_rcl_node_options(); ASSERT_TRUE(rcl_options != nullptr); @@ -61,22 +70,25 @@ TEST(TestNodeOptions, ros_args_and_non_ros_args) { ASSERT_EQ(2, rcl_arguments_get_count_unparsed(&rcl_options->arguments)); char * node_name = nullptr; - EXPECT_EQ(RCL_RET_OK, rcl_remap_node_name( - &rcl_options->arguments, nullptr, "my_node", allocator, &node_name)); + EXPECT_EQ( + RCL_RET_OK, + rcl_remap_node_name(&rcl_options->arguments, nullptr, "my_node", allocator, &node_name)); ASSERT_TRUE(node_name != nullptr); EXPECT_STREQ("some_node", node_name); allocator.deallocate(node_name, allocator.state); char * namespace_name = nullptr; - EXPECT_EQ(RCL_RET_OK, rcl_remap_node_namespace( + EXPECT_EQ( + RCL_RET_OK, + rcl_remap_node_namespace( &rcl_options->arguments, nullptr, "my_ns", allocator, &namespace_name)); ASSERT_TRUE(namespace_name != nullptr); EXPECT_STREQ("/some_ns", namespace_name); allocator.deallocate(namespace_name, allocator.state); int * output_indices = nullptr; - EXPECT_EQ(RCL_RET_OK, rcl_arguments_get_unparsed( - &rcl_options->arguments, allocator, &output_indices)); + EXPECT_EQ( + RCL_RET_OK, rcl_arguments_get_unparsed(&rcl_options->arguments, allocator, &output_indices)); ASSERT_TRUE(output_indices != nullptr); const std::vector & args = options.arguments(); EXPECT_EQ("--non-ros-flag", args[output_indices[0]]); @@ -84,17 +96,13 @@ TEST(TestNodeOptions, ros_args_and_non_ros_args) { allocator.deallocate(output_indices, allocator.state); } -TEST(TestNodeOptions, bad_ros_args) { +TEST(TestNodeOptions, bad_ros_args) +{ rcl_allocator_t allocator = rcl_get_default_allocator(); - auto options = rclcpp::NodeOptions(allocator) - .arguments({"--ros-args", "-r", "foo:="}); + auto options = rclcpp::NodeOptions(allocator).arguments({"--ros-args", "-r", "foo:="}); - EXPECT_THROW( - options.get_rcl_node_options(), - rclcpp::exceptions::RCLInvalidROSArgsError); + EXPECT_THROW(options.get_rcl_node_options(), rclcpp::exceptions::RCLInvalidROSArgsError); options.arguments({"--ros-args", "-r", "foo:=bar", "not-a-ros-arg"}); - EXPECT_THROW( - options.get_rcl_node_options(), - rclcpp::exceptions::UnknownROSArgsError); + EXPECT_THROW(options.get_rcl_node_options(), rclcpp::exceptions::UnknownROSArgsError); } diff --git a/rclcpp/test/test_parameter.cpp b/rclcpp/test/test_parameter.cpp index 876fbb4e93..8382f7b04d 100644 --- a/rclcpp/test/test_parameter.cpp +++ b/rclcpp/test/test_parameter.cpp @@ -32,7 +32,8 @@ class TestParameter : public ::testing::Test } }; -TEST(TestParameter, not_set_variant) { +TEST(TestParameter, not_set_variant) +{ // Direct instantiation rclcpp::Parameter not_set_variant; EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, not_set_variant.get_type()); @@ -58,7 +59,8 @@ TEST(TestParameter, not_set_variant) { rclcpp::Parameter::from_parameter_msg(not_set_param).get_type()); } -TEST(TestParameter, bool_variant) { +TEST(TestParameter, bool_variant) +{ // Direct instantiation rclcpp::Parameter bool_variant_true("bool_param", true); EXPECT_EQ("bool_param", bool_variant_true.get_name()); @@ -67,8 +69,7 @@ TEST(TestParameter, bool_variant) { EXPECT_TRUE(bool_variant_true.get_value()); EXPECT_TRUE(bool_variant_true.get_value_message().bool_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, - bool_variant_true.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, bool_variant_true.get_value_message().type); EXPECT_TRUE(bool_variant_true.as_bool()); EXPECT_THROW(bool_variant_true.as_int(), rclcpp::ParameterTypeException); @@ -95,8 +96,7 @@ TEST(TestParameter, bool_variant) { EXPECT_TRUE(bool_param.value.bool_value); // From parameter message - rclcpp::Parameter from_msg_true = - rclcpp::Parameter::from_parameter_msg(bool_param); + rclcpp::Parameter from_msg_true = rclcpp::Parameter::from_parameter_msg(bool_param); EXPECT_EQ("bool_param", from_msg_true.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL, from_msg_true.get_type()); EXPECT_EQ("bool", from_msg_true.get_type_name()); @@ -107,8 +107,7 @@ TEST(TestParameter, bool_variant) { bool_variant_false.get_value_message().type); bool_param.value.bool_value = false; - rclcpp::Parameter from_msg_false = - rclcpp::Parameter::from_parameter_msg(bool_param); + rclcpp::Parameter from_msg_false = rclcpp::Parameter::from_parameter_msg(bool_param); EXPECT_FALSE(from_msg_false.get_value()); EXPECT_FALSE(from_msg_false.get_value_message().bool_value); EXPECT_EQ( @@ -116,17 +115,16 @@ TEST(TestParameter, bool_variant) { bool_variant_false.get_value_message().type); } -TEST(TestParameter, integer_variant) { - const int TEST_VALUE {42}; +TEST(TestParameter, integer_variant) +{ + const int TEST_VALUE{42}; // Direct instantiation rclcpp::Parameter integer_variant("integer_param", TEST_VALUE); EXPECT_EQ("integer_param", integer_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, integer_variant.get_type()); EXPECT_EQ("integer", integer_variant.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - integer_variant.get_value()); + EXPECT_EQ(TEST_VALUE, integer_variant.get_value()); EXPECT_EQ(TEST_VALUE, integer_variant.get_value_message().integer_value); EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, @@ -150,35 +148,29 @@ TEST(TestParameter, integer_variant) { EXPECT_EQ(TEST_VALUE, integer_param.value.integer_value); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(integer_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(integer_param); EXPECT_EQ("integer_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, from_msg.get_type()); EXPECT_EQ("integer", from_msg.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, - from_msg.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, from_msg.get_value_message().type); } -TEST(TestParameter, long_integer_variant) { - const int64_t TEST_VALUE {std::numeric_limits::max()}; +TEST(TestParameter, long_integer_variant) +{ + const int64_t TEST_VALUE{std::numeric_limits::max()}; // Direct instantiation rclcpp::Parameter long_variant("long_integer_param", TEST_VALUE); EXPECT_EQ("long_integer_param", long_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, long_variant.get_type()); EXPECT_EQ("integer", long_variant.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - long_variant.get_value()); + EXPECT_EQ(TEST_VALUE, long_variant.get_value()); EXPECT_EQ(TEST_VALUE, long_variant.get_value_message().integer_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, - long_variant.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, long_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, long_variant.as_int()); EXPECT_THROW(long_variant.as_bool(), rclcpp::ParameterTypeException); @@ -198,35 +190,29 @@ TEST(TestParameter, long_integer_variant) { EXPECT_EQ(TEST_VALUE, integer_param.value.integer_value); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(integer_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(integer_param); EXPECT_EQ("long_integer_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, from_msg.get_type()); EXPECT_EQ("integer", from_msg.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, - from_msg.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, from_msg.get_value_message().type); } -TEST(TestParameter, float_variant) { - const float TEST_VALUE {42.0f}; +TEST(TestParameter, float_variant) +{ + const float TEST_VALUE{42.0f}; // Direct instantiation rclcpp::Parameter float_variant("float_param", TEST_VALUE); EXPECT_EQ("float_param", float_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, float_variant.get_type()); EXPECT_EQ("double", float_variant.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - float_variant.get_value()); + EXPECT_EQ(TEST_VALUE, float_variant.get_value()); EXPECT_EQ(TEST_VALUE, float_variant.get_value_message().double_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, - float_variant.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, float_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, float_variant.as_double()); EXPECT_THROW(float_variant.as_bool(), rclcpp::ParameterTypeException); @@ -246,35 +232,29 @@ TEST(TestParameter, float_variant) { EXPECT_EQ(TEST_VALUE, float_param.value.double_value); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(float_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(float_param); EXPECT_EQ("float_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, from_msg.get_type()); EXPECT_EQ("double", from_msg.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, - from_msg.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, from_msg.get_value_message().type); } -TEST(TestParameter, double_variant) { - const double TEST_VALUE {-42.1}; +TEST(TestParameter, double_variant) +{ + const double TEST_VALUE{-42.1}; // Direct instantiation rclcpp::Parameter double_variant("double_param", TEST_VALUE); EXPECT_EQ("double_param", double_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, double_variant.get_type()); EXPECT_EQ("double", double_variant.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - double_variant.get_value()); + EXPECT_EQ(TEST_VALUE, double_variant.get_value()); EXPECT_EQ(TEST_VALUE, double_variant.get_value_message().double_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, - double_variant.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, double_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, double_variant.as_double()); EXPECT_THROW(double_variant.as_bool(), rclcpp::ParameterTypeException); @@ -294,35 +274,29 @@ TEST(TestParameter, double_variant) { EXPECT_EQ(TEST_VALUE, double_param.value.double_value); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(double_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(double_param); EXPECT_EQ("double_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, from_msg.get_type()); EXPECT_EQ("double", from_msg.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, - from_msg.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, from_msg.get_value_message().type); } -TEST(TestParameter, string_variant) { - const std::string TEST_VALUE {"ROS2"}; +TEST(TestParameter, string_variant) +{ + const std::string TEST_VALUE{"ROS2"}; // Direct instantiation rclcpp::Parameter string_variant("string_param", TEST_VALUE); EXPECT_EQ("string_param", string_variant.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING, string_variant.get_type()); EXPECT_EQ("string", string_variant.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - string_variant.get_value()); + EXPECT_EQ(TEST_VALUE, string_variant.get_value()); EXPECT_EQ(TEST_VALUE, string_variant.get_value_message().string_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_STRING, - string_variant.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_STRING, string_variant.get_value_message().type); EXPECT_EQ(TEST_VALUE, string_variant.as_string()); EXPECT_THROW(string_variant.as_bool(), rclcpp::ParameterTypeException); @@ -342,20 +316,19 @@ TEST(TestParameter, string_variant) { EXPECT_EQ(TEST_VALUE, string_param.value.string_value); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(string_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(string_param); EXPECT_EQ("string_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING, from_msg.get_type()); EXPECT_EQ("string", from_msg.get_type_name()); EXPECT_EQ(TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().string_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_STRING, - from_msg.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_STRING, from_msg.get_value_message().type); } -TEST(TestParameter, byte_array_variant) { - const std::vector TEST_VALUE {0x52, 0x4f, 0x53, 0x32}; +TEST(TestParameter, byte_array_variant) +{ + const std::vector TEST_VALUE{0x52, 0x4f, 0x53, 0x32}; // Direct instantiation rclcpp::Parameter byte_array_variant("byte_array_param", TEST_VALUE); @@ -363,8 +336,7 @@ TEST(TestParameter, byte_array_variant) { EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BYTE_ARRAY, byte_array_variant.get_type()); EXPECT_EQ("byte_array", byte_array_variant.get_type_name()); EXPECT_EQ( - TEST_VALUE, - byte_array_variant.get_value()); + TEST_VALUE, byte_array_variant.get_value()); EXPECT_EQ(TEST_VALUE, byte_array_variant.get_value_message().byte_array_value); EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, @@ -388,22 +360,19 @@ TEST(TestParameter, byte_array_variant) { EXPECT_EQ(TEST_VALUE, byte_array_param.value.byte_array_value); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(byte_array_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(byte_array_param); EXPECT_EQ("byte_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BYTE_ARRAY, from_msg.get_type()); EXPECT_EQ("byte_array", from_msg.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().byte_array_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, - from_msg.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, from_msg.get_value_message().type); } -TEST(TestParameter, bool_array_variant) { - const std::vector TEST_VALUE {false, true, true, false, false, true}; +TEST(TestParameter, bool_array_variant) +{ + const std::vector TEST_VALUE{false, true, true, false, false, true}; // Direct instantiation rclcpp::Parameter bool_array_variant("bool_array_param", TEST_VALUE); @@ -411,8 +380,7 @@ TEST(TestParameter, bool_array_variant) { EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL_ARRAY, bool_array_variant.get_type()); EXPECT_EQ("bool_array", bool_array_variant.get_type_name()); EXPECT_EQ( - TEST_VALUE, - bool_array_variant.get_value()); + TEST_VALUE, bool_array_variant.get_value()); EXPECT_EQ(TEST_VALUE, bool_array_variant.get_value_message().bool_array_value); EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, @@ -436,31 +404,26 @@ TEST(TestParameter, bool_array_variant) { EXPECT_EQ(TEST_VALUE, bool_array_param.value.bool_array_value); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(bool_array_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(bool_array_param); EXPECT_EQ("bool_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL_ARRAY, from_msg.get_type()); EXPECT_EQ("bool_array", from_msg.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().bool_array_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, - from_msg.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, from_msg.get_value_message().type); } -TEST(TestParameter, integer_array_variant) { - const std::vector TEST_VALUE - {42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; +TEST(TestParameter, integer_array_variant) +{ + const std::vector TEST_VALUE{ + 42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; // Direct instantiation rclcpp::Parameter integer_array_variant("integer_array_param", TEST_VALUE); EXPECT_EQ("integer_array_param", integer_array_variant.get_name()); - EXPECT_EQ( - rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, - integer_array_variant.get_type()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, integer_array_variant.get_type()); EXPECT_EQ("integer_array", integer_array_variant.get_type_name()); EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, @@ -492,15 +455,12 @@ TEST(TestParameter, integer_array_variant) { EXPECT_THROW(integer_array_variant.as_double_array(), rclcpp::ParameterTypeException); EXPECT_THROW(integer_array_variant.as_string_array(), rclcpp::ParameterTypeException); - EXPECT_EQ( - "[42, -99, 2147483647, -2147483648, 0]", - integer_array_variant.value_to_string()); + EXPECT_EQ("[42, -99, 2147483647, -2147483648, 0]", integer_array_variant.value_to_string()); rcl_interfaces::msg::Parameter integer_array_param = integer_array_variant.to_parameter_msg(); EXPECT_EQ("integer_array_param", integer_array_param.name); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, - integer_array_param.value.type); + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, integer_array_param.value.type); param_value = integer_array_param.value.integer_array_value; mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); @@ -508,8 +468,7 @@ TEST(TestParameter, integer_array_variant) { EXPECT_EQ(param_value.end(), mismatches.second); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(integer_array_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(integer_array_param); EXPECT_EQ("integer_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_type()); EXPECT_EQ("integer_array", from_msg.get_type_name()); @@ -525,26 +484,23 @@ TEST(TestParameter, integer_array_variant) { EXPECT_EQ(param_value.end(), mismatches.second); EXPECT_EQ( - from_msg.get_value_message().type, - rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + from_msg.get_value_message().type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); } -TEST(TestParameter, long_integer_array_variant) { - const std::vector TEST_VALUE - {42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; +TEST(TestParameter, long_integer_array_variant) +{ + const std::vector TEST_VALUE{ + 42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; rclcpp::Parameter long_array_variant("long_integer_array_param", TEST_VALUE); EXPECT_EQ("long_integer_array_param", long_array_variant.get_name()); - EXPECT_EQ( - rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, - long_array_variant.get_type()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, long_array_variant.get_type()); EXPECT_EQ("integer_array", long_array_variant.get_type_name()); EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, long_array_variant.get_value_message().type); EXPECT_EQ( - TEST_VALUE, - long_array_variant.get_value()); + TEST_VALUE, long_array_variant.get_value()); EXPECT_EQ(TEST_VALUE, long_array_variant.get_value_message().integer_array_value); EXPECT_EQ(TEST_VALUE, long_array_variant.as_integer_array()); @@ -564,36 +520,30 @@ TEST(TestParameter, long_integer_array_variant) { rcl_interfaces::msg::Parameter integer_array_param = long_array_variant.to_parameter_msg(); EXPECT_EQ("long_integer_array_param", integer_array_param.name); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, - integer_array_param.value.type); + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, integer_array_param.value.type); EXPECT_EQ(TEST_VALUE, integer_array_param.value.integer_array_value); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(integer_array_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(integer_array_param); EXPECT_EQ("long_integer_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_type()); EXPECT_EQ("integer_array", from_msg.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_array_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, - from_msg.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_value_message().type); } -TEST(TestParameter, float_array_variant) { - const std::vector TEST_VALUE - {42.1f, -99.1f, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1f}; +TEST(TestParameter, float_array_variant) +{ + const std::vector TEST_VALUE{ + 42.1f, -99.1f, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1f}; // Direct instantiation rclcpp::Parameter float_array_variant("float_array_param", TEST_VALUE); EXPECT_EQ("float_array_param", float_array_variant.get_name()); - EXPECT_EQ( - rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, - float_array_variant.get_type()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, float_array_variant.get_type()); EXPECT_EQ("double_array", float_array_variant.get_type_name()); EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, @@ -625,15 +575,12 @@ TEST(TestParameter, float_array_variant) { EXPECT_THROW(float_array_variant.as_integer_array(), rclcpp::ParameterTypeException); EXPECT_THROW(float_array_variant.as_string_array(), rclcpp::ParameterTypeException); - EXPECT_EQ( - "[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]", - float_array_variant.value_to_string()); + EXPECT_EQ("[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]", float_array_variant.value_to_string()); rcl_interfaces::msg::Parameter float_array_param = float_array_variant.to_parameter_msg(); EXPECT_EQ("float_array_param", float_array_param.name); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, - float_array_param.value.type); + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, float_array_param.value.type); param_value = float_array_param.value.double_array_value; mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); @@ -641,8 +588,7 @@ TEST(TestParameter, float_array_variant) { EXPECT_EQ(param_value.end(), mismatches.second); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(float_array_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(float_array_param); EXPECT_EQ("float_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_type()); EXPECT_EQ("double_array", from_msg.get_type_name()); @@ -658,26 +604,23 @@ TEST(TestParameter, float_array_variant) { EXPECT_EQ(param_value.end(), mismatches.second); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, - from_msg.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_value_message().type); } -TEST(TestParameter, double_array_variant) { - const std::vector TEST_VALUE - {42.1, -99.1, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1}; +TEST(TestParameter, double_array_variant) +{ + const std::vector TEST_VALUE{ + 42.1, -99.1, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1}; rclcpp::Parameter double_array_variant("double_array_param", TEST_VALUE); EXPECT_EQ("double_array_param", double_array_variant.get_name()); - EXPECT_EQ( - rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, - double_array_variant.get_type()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, double_array_variant.get_type()); EXPECT_EQ("double_array", double_array_variant.get_type_name()); EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, double_array_variant.get_value_message().type); EXPECT_EQ( - TEST_VALUE, - double_array_variant.get_value()); + TEST_VALUE, double_array_variant.get_value()); EXPECT_EQ(TEST_VALUE, double_array_variant.get_value_message().double_array_value); EXPECT_EQ(TEST_VALUE, double_array_variant.as_double_array()); @@ -691,44 +634,36 @@ TEST(TestParameter, double_array_variant) { EXPECT_THROW(double_array_variant.as_string_array(), rclcpp::ParameterTypeException); EXPECT_EQ( - "[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]", - double_array_variant.value_to_string()); + "[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]", double_array_variant.value_to_string()); rcl_interfaces::msg::Parameter double_array_param = double_array_variant.to_parameter_msg(); EXPECT_EQ("double_array_param", double_array_param.name); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, - double_array_param.value.type); + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, double_array_param.value.type); EXPECT_EQ(TEST_VALUE, double_array_param.value.double_array_value); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(double_array_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(double_array_param); EXPECT_EQ("double_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_type()); EXPECT_EQ("double_array", from_msg.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_array_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, - from_msg.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_value_message().type); } -TEST(TestParameter, string_array_variant) { - const std::vector TEST_VALUE {"R", "O", "S2"}; +TEST(TestParameter, string_array_variant) +{ + const std::vector TEST_VALUE{"R", "O", "S2"}; // Direct instantiation rclcpp::Parameter string_array_variant("string_array_param", TEST_VALUE); EXPECT_EQ("string_array_param", string_array_variant.get_name()); - EXPECT_EQ( - rclcpp::ParameterType::PARAMETER_STRING_ARRAY, - string_array_variant.get_type()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING_ARRAY, string_array_variant.get_type()); EXPECT_EQ("string_array", string_array_variant.get_type_name()); EXPECT_EQ( - TEST_VALUE, - string_array_variant.get_value()); + TEST_VALUE, string_array_variant.get_value()); EXPECT_EQ(TEST_VALUE, string_array_variant.get_value_message().string_array_value); EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, @@ -749,21 +684,16 @@ TEST(TestParameter, string_array_variant) { rcl_interfaces::msg::Parameter string_array_param = string_array_variant.to_parameter_msg(); EXPECT_EQ("string_array_param", string_array_param.name); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, - string_array_param.value.type); + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, string_array_param.value.type); EXPECT_EQ(TEST_VALUE, string_array_param.value.string_array_value); // From parameter message - rclcpp::Parameter from_msg = - rclcpp::Parameter::from_parameter_msg(string_array_param); + rclcpp::Parameter from_msg = rclcpp::Parameter::from_parameter_msg(string_array_param); EXPECT_EQ("string_array_param", from_msg.get_name()); EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING_ARRAY, from_msg.get_type()); EXPECT_EQ("string_array", from_msg.get_type_name()); - EXPECT_EQ( - TEST_VALUE, - from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value()); EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().string_array_value); EXPECT_EQ( - rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, - from_msg.get_value_message().type); + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, from_msg.get_value_message().type); } diff --git a/rclcpp/test/test_parameter_client.cpp b/rclcpp/test/test_parameter_client.cpp index 2630353ffa..8e9031c7ae 100644 --- a/rclcpp/test/test_parameter_client.cpp +++ b/rclcpp/test/test_parameter_client.cpp @@ -14,8 +14,8 @@ #include -#include #include +#include #include "rclcpp/rclcpp.hpp" @@ -51,7 +51,8 @@ class TestParameterClient : public ::testing::Test /* Testing async parameter client construction and destruction. */ -TEST_F(TestParameterClient, async_construction_and_destruction) { +TEST_F(TestParameterClient, async_construction_and_destruction) +{ { auto asynchronous_client = std::make_shared(node); (void)asynchronous_client; @@ -68,16 +69,16 @@ TEST_F(TestParameterClient, async_construction_and_destruction) { { ASSERT_THROW( - { - std::make_shared(node, "invalid_remote_node?"); - }, rclcpp::exceptions::InvalidServiceNameError); + { std::make_shared(node, "invalid_remote_node?"); }, + rclcpp::exceptions::InvalidServiceNameError); } } /* Testing sync parameter client construction and destruction. */ -TEST_F(TestParameterClient, sync_construction_and_destruction) { +TEST_F(TestParameterClient, sync_construction_and_destruction) +{ { auto synchronous_client = std::make_shared(node); (void)synchronous_client; @@ -85,8 +86,7 @@ TEST_F(TestParameterClient, sync_construction_and_destruction) { { auto synchronous_client = std::make_shared( - std::make_shared(), - node); + std::make_shared(), node); (void)synchronous_client; } @@ -102,16 +102,16 @@ TEST_F(TestParameterClient, sync_construction_and_destruction) { { ASSERT_THROW( - { - std::make_shared(node, "invalid_remote_node?"); - }, rclcpp::exceptions::InvalidServiceNameError); + { std::make_shared(node, "invalid_remote_node?"); }, + rclcpp::exceptions::InvalidServiceNameError); } } /* Testing different methods for parameter event subscription from asynchronous clients. */ -TEST_F(TestParameterClient, async_parameter_event_subscription) { +TEST_F(TestParameterClient, async_parameter_event_subscription) +{ auto callback = std::bind(&TestParameterClient::OnMessage, this, std::placeholders::_1); { auto asynchronous_client = std::make_shared(node); @@ -126,8 +126,7 @@ TEST_F(TestParameterClient, async_parameter_event_subscription) { { auto event_sub = rclcpp::AsyncParametersClient::on_parameter_event( - node->get_node_topics_interface(), - callback); + node->get_node_topics_interface(), callback); (void)event_sub; } } @@ -135,7 +134,8 @@ TEST_F(TestParameterClient, async_parameter_event_subscription) { /* Testing different methods for parameter event subscription from synchronous clients. */ -TEST_F(TestParameterClient, sync_parameter_event_subscription) { +TEST_F(TestParameterClient, sync_parameter_event_subscription) +{ auto callback = std::bind(&TestParameterClient::OnMessage, this, std::placeholders::_1); { auto synchronous_client = std::make_shared(node); @@ -149,9 +149,8 @@ TEST_F(TestParameterClient, sync_parameter_event_subscription) { } { - auto event_sub = rclcpp::SyncParametersClient::on_parameter_event( - node->get_node_topics_interface(), - callback); + auto event_sub = + rclcpp::SyncParametersClient::on_parameter_event(node->get_node_topics_interface(), callback); (void)event_sub; } } diff --git a/rclcpp/test/test_parameter_events_filter.cpp b/rclcpp/test/test_parameter_events_filter.cpp index a497a28c93..4be9c96fd3 100644 --- a/rclcpp/test/test_parameter_events_filter.cpp +++ b/rclcpp/test/test_parameter_events_filter.cpp @@ -14,8 +14,8 @@ #include -#include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/parameter_events_filter.hpp" @@ -72,131 +72,71 @@ class TestParameterEventFilter : public ::testing::Test /* Testing filters. */ -TEST_F(TestParameterEventFilter, full_by_type) { - auto res = rclcpp::ParameterEventsFilter( - full, - {"new", "changed", "deleted"}, - {nt, ct, dt}); +TEST_F(TestParameterEventFilter, full_by_type) +{ + auto res = rclcpp::ParameterEventsFilter(full, {"new", "changed", "deleted"}, {nt, ct, dt}); EXPECT_EQ(3u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"new", "changed", "deleted"}, - {nt, ct}); + res = rclcpp::ParameterEventsFilter(full, {"new", "changed", "deleted"}, {nt, ct}); EXPECT_EQ(2u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"new", "changed", "deleted"}, - {nt, dt}); + res = rclcpp::ParameterEventsFilter(full, {"new", "changed", "deleted"}, {nt, dt}); EXPECT_EQ(2u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"new", "changed", "deleted"}, - {ct, dt}); + res = rclcpp::ParameterEventsFilter(full, {"new", "changed", "deleted"}, {ct, dt}); EXPECT_EQ(2u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"new", "changed", "deleted"}, - {nt}); + res = rclcpp::ParameterEventsFilter(full, {"new", "changed", "deleted"}, {nt}); EXPECT_EQ(1u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"new", "changed", "deleted"}, - {ct}); + res = rclcpp::ParameterEventsFilter(full, {"new", "changed", "deleted"}, {ct}); EXPECT_EQ(1u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"new", "changed", "deleted"}, - {dt}); + res = rclcpp::ParameterEventsFilter(full, {"new", "changed", "deleted"}, {dt}); EXPECT_EQ(1u, res.get_events().size()); } -TEST_F(TestParameterEventFilter, full_by_name) { - auto res = rclcpp::ParameterEventsFilter( - full, - {"new", "changed", "deleted"}, - {nt, ct, dt}); +TEST_F(TestParameterEventFilter, full_by_name) +{ + auto res = rclcpp::ParameterEventsFilter(full, {"new", "changed", "deleted"}, {nt, ct, dt}); EXPECT_EQ(3u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"new", "changed"}, - {nt, ct, dt}); + res = rclcpp::ParameterEventsFilter(full, {"new", "changed"}, {nt, ct, dt}); EXPECT_EQ(2u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"new", "deleted"}, - {nt, ct, dt}); + res = rclcpp::ParameterEventsFilter(full, {"new", "deleted"}, {nt, ct, dt}); EXPECT_EQ(2u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"changed", "deleted"}, - {nt, ct, dt}); + res = rclcpp::ParameterEventsFilter(full, {"changed", "deleted"}, {nt, ct, dt}); EXPECT_EQ(2u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"new"}, - {nt, ct, dt}); + res = rclcpp::ParameterEventsFilter(full, {"new"}, {nt, ct, dt}); EXPECT_EQ(1u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"changed"}, - {nt, ct, dt}); + res = rclcpp::ParameterEventsFilter(full, {"changed"}, {nt, ct, dt}); EXPECT_EQ(1u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - full, - {"deleted"}, - {nt, ct, dt}); + res = rclcpp::ParameterEventsFilter(full, {"deleted"}, {nt, ct, dt}); EXPECT_EQ(1u, res.get_events().size()); } -TEST_F(TestParameterEventFilter, empty) { - auto res = rclcpp::ParameterEventsFilter( - empty, - {"new", "changed", "deleted"}, - {nt, ct, dt}); +TEST_F(TestParameterEventFilter, empty) +{ + auto res = rclcpp::ParameterEventsFilter(empty, {"new", "changed", "deleted"}, {nt, ct, dt}); EXPECT_EQ(0u, res.get_events().size()); } -TEST_F(TestParameterEventFilter, singular) { - auto res = rclcpp::ParameterEventsFilter( - np, - {"new", "changed", "deleted"}, - {nt, ct, dt}); +TEST_F(TestParameterEventFilter, singular) +{ + auto res = rclcpp::ParameterEventsFilter(np, {"new", "changed", "deleted"}, {nt, ct, dt}); EXPECT_EQ(1u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - cp, - {"new", "changed", "deleted"}, - {nt, ct, dt}); + res = rclcpp::ParameterEventsFilter(cp, {"new", "changed", "deleted"}, {nt, ct, dt}); EXPECT_EQ(1u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - dp, - {"new", "changed", "deleted"}, - {nt, ct, dt}); + res = rclcpp::ParameterEventsFilter(dp, {"new", "changed", "deleted"}, {nt, ct, dt}); EXPECT_EQ(1u, res.get_events().size()); } -TEST_F(TestParameterEventFilter, multiple) { - auto res = rclcpp::ParameterEventsFilter( - multiple, - {"new", "new2"}, - {nt, ct, dt}); +TEST_F(TestParameterEventFilter, multiple) +{ + auto res = rclcpp::ParameterEventsFilter(multiple, {"new", "new2"}, {nt, ct, dt}); EXPECT_EQ(2u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - multiple, - {"new2"}, - {nt, ct, dt}); + res = rclcpp::ParameterEventsFilter(multiple, {"new2"}, {nt, ct, dt}); EXPECT_EQ(1u, res.get_events().size()); - res = rclcpp::ParameterEventsFilter( - multiple, - {"new", "new2"}, - {ct, dt}); + res = rclcpp::ParameterEventsFilter(multiple, {"new", "new2"}, {ct, dt}); EXPECT_EQ(0u, res.get_events().size()); } -TEST_F(TestParameterEventFilter, validate_data) { - auto res = rclcpp::ParameterEventsFilter( - multiple, - {"new", "new2"}, - {nt, ct, dt}); +TEST_F(TestParameterEventFilter, validate_data) +{ + auto res = rclcpp::ParameterEventsFilter(multiple, {"new", "new2"}, {nt, ct, dt}); EXPECT_EQ(2u, res.get_events().size()); EXPECT_EQ(nt, res.get_events()[0].first); EXPECT_EQ(nt, res.get_events()[1].first); diff --git a/rclcpp/test/test_parameter_map.cpp b/rclcpp/test/test_parameter_map.cpp index 3f54e4c879..a018133f2f 100644 --- a/rclcpp/test/test_parameter_map.cpp +++ b/rclcpp/test/test_parameter_map.cpp @@ -23,8 +23,7 @@ #include "rclcpp/parameter_map.hpp" -rcl_params_t * -make_params(std::vector node_names) +rcl_params_t * make_params(std::vector node_names) { rcl_allocator_t alloc = rcl_get_default_allocator(); rcl_params_t * c_params = rcl_yaml_node_struct_init(alloc); @@ -33,8 +32,8 @@ make_params(std::vector node_names) if (c_params->num_nodes) { // Copy node names for (size_t n = 0; n < node_names.size(); ++n) { - c_params->node_names[n] = static_cast(alloc.allocate( - sizeof(char) * (node_names[n].size() + 1), alloc.state)); + c_params->node_names[n] = + static_cast(alloc.allocate(sizeof(char) * (node_names[n].size() + 1), alloc.state)); std::snprintf(c_params->node_names[n], node_names[n].size() + 1, "%s", node_names[n].c_str()); } // zero init node params @@ -47,8 +46,8 @@ make_params(std::vector node_names) return c_params; } -void -make_node_params(rcl_params_t * c_params, size_t node_idx, std::vector param_names) +void make_node_params( + rcl_params_t * c_params, size_t node_idx, std::vector param_names) { rcl_allocator_t alloc = c_params->allocator; ASSERT_LT(node_idx, c_params->num_nodes); @@ -58,18 +57,18 @@ make_node_params(rcl_params_t * c_params, size_t node_idx, std::vectornum_params = param_names.size(); // Copy parameter names - c_node_params->parameter_names = static_cast( - alloc.allocate(sizeof(char *) * param_names.size(), alloc.state)); + c_node_params->parameter_names = + static_cast(alloc.allocate(sizeof(char *) * param_names.size(), alloc.state)); for (size_t p = 0; p < param_names.size(); ++p) { const std::string & param_name = param_names[p]; - c_node_params->parameter_names[p] = static_cast(alloc.allocate( - sizeof(char) * (param_name.size() + 1), alloc.state)); + c_node_params->parameter_names[p] = + static_cast(alloc.allocate(sizeof(char) * (param_name.size() + 1), alloc.state)); std::snprintf( c_node_params->parameter_names[p], param_name.size() + 1, "%s", param_name.c_str()); } // zero init parameter value - c_node_params->parameter_values = static_cast(alloc.allocate( - sizeof(rcl_variant_t) * param_names.size(), alloc.state)); + c_node_params->parameter_values = static_cast( + alloc.allocate(sizeof(rcl_variant_t) * param_names.size(), alloc.state)); for (size_t p = 0; p < param_names.size(); ++p) { c_node_params->parameter_values[p].bool_value = NULL; c_node_params->parameter_values[p].integer_value = NULL; @@ -235,16 +234,16 @@ TEST(Test_parameter_map_from, string_param_value) } #define MAKE_ARRAY_VALUE(VAR, TYPE, V1, V2) \ - do { \ - VAR.values = new TYPE[2]; \ - VAR.size = 2; \ - VAR.values[0] = V1; \ - VAR.values[1] = V2; \ + do { \ + VAR.values = new TYPE[2]; \ + VAR.size = 2; \ + VAR.values[0] = V1; \ + VAR.values[1] = V2; \ } while (false) #define FREE_ARRAY_VALUE(VAR) \ - do { \ - delete[] VAR.values; \ + do { \ + delete[] VAR.values; \ } while (false) TEST(Test_parameter_map_from, byte_array_param_value) diff --git a/rclcpp/test/test_publisher.cpp b/rclcpp/test/test_publisher.cpp index d86fdfcc65..9059600acd 100644 --- a/rclcpp/test/test_publisher.cpp +++ b/rclcpp/test/test_publisher.cpp @@ -14,8 +14,8 @@ #include -#include #include +#include #include #include "rclcpp/exceptions.hpp" @@ -47,8 +47,9 @@ class TestPublisher : public ::testing::Test struct TestParameters { - TestParameters(rclcpp::QoS qos, std::string description) - : qos(qos), description(description) {} + TestParameters(rclcpp::QoS qos, std::string description) : qos(qos), description(description) + { + } rclcpp::QoS qos; std::string description; }; @@ -59,10 +60,10 @@ std::ostream & operator<<(std::ostream & out, const TestParameters & params) return out; } -class TestPublisherInvalidIntraprocessQos - : public TestPublisher, - public ::testing::WithParamInterface -{}; +class TestPublisherInvalidIntraprocessQos : public TestPublisher, + public ::testing::WithParamInterface +{ +}; class TestPublisherSub : public ::testing::Test { @@ -89,7 +90,8 @@ class TestPublisherSub : public ::testing::Test /* Testing publisher construction and destruction. */ -TEST_F(TestPublisher, construction_and_destruction) { +TEST_F(TestPublisher, construction_and_destruction) +{ initialize(); using rcl_interfaces::msg::IntraProcessMessage; { @@ -99,16 +101,16 @@ TEST_F(TestPublisher, construction_and_destruction) { { ASSERT_THROW( - { - auto publisher = node->create_publisher("invalid_topic?", 42); - }, rclcpp::exceptions::InvalidTopicNameError); + { auto publisher = node->create_publisher("invalid_topic?", 42); }, + rclcpp::exceptions::InvalidTopicNameError); } } /* Testing publisher creation signatures. */ -TEST_F(TestPublisher, various_creation_signatures) { +TEST_F(TestPublisher, various_creation_signatures) +{ initialize(); using rcl_interfaces::msg::IntraProcessMessage; { @@ -144,13 +146,14 @@ TEST_F(TestPublisher, various_creation_signatures) { /* Testing publisher with intraprocess enabled and invalid QoS */ -TEST_P(TestPublisherInvalidIntraprocessQos, test_publisher_throws) { +TEST_P(TestPublisherInvalidIntraprocessQos, test_publisher_throws) +{ initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); rclcpp::QoS qos = GetParam().qos; using rcl_interfaces::msg::IntraProcessMessage; { ASSERT_THROW( - {auto publisher = node->create_publisher("topic", qos);}, + { auto publisher = node->create_publisher("topic", qos); }, std::invalid_argument); } } @@ -161,30 +164,25 @@ static std::vector invalid_qos_profiles() parameters.reserve(3); parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), - "transient_local_qos")); + TestParameters(rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), "transient_local_qos")); parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepLast(0)), - "keep_last_qos_with_zero_history_depth")); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepAll()), - "keep_all_qos")); + TestParameters(rclcpp::QoS(rclcpp::KeepLast(0)), "keep_last_qos_with_zero_history_depth")); + parameters.push_back(TestParameters(rclcpp::QoS(rclcpp::KeepAll()), "keep_all_qos")); return parameters; } INSTANTIATE_TEST_CASE_P( - TestPublisherThrows, TestPublisherInvalidIntraprocessQos, + TestPublisherThrows, + TestPublisherInvalidIntraprocessQos, ::testing::ValuesIn(invalid_qos_profiles()), ::testing::PrintToStringParamName()); /* Testing publisher construction and destruction for subnodes. */ -TEST_F(TestPublisherSub, construction_and_destruction) { +TEST_F(TestPublisherSub, construction_and_destruction) +{ using rcl_interfaces::msg::IntraProcessMessage; { auto publisher = subnode->create_publisher("topic", 42); @@ -200,8 +198,7 @@ TEST_F(TestPublisherSub, construction_and_destruction) { { ASSERT_THROW( - { - auto publisher = subnode->create_publisher("invalid_topic?", 42); - }, rclcpp::exceptions::InvalidTopicNameError); + { auto publisher = subnode->create_publisher("invalid_topic?", 42); }, + rclcpp::exceptions::InvalidTopicNameError); } } diff --git a/rclcpp/test/test_publisher_subscription_count_api.cpp b/rclcpp/test/test_publisher_subscription_count_api.cpp index d92f8f3dd4..70de49087d 100644 --- a/rclcpp/test/test_publisher_subscription_count_api.cpp +++ b/rclcpp/test/test_publisher_subscription_count_api.cpp @@ -15,8 +15,8 @@ #include #include -#include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/publisher.hpp" @@ -53,9 +53,13 @@ class TestPublisherSubscriptionCount : public ::testing::TestWithParam( - "my_node", - "/ns", - parameters.node_options[0]); + rclcpp::Node::SharedPtr node = + std::make_shared("my_node", "/ns", parameters.node_options[0]); auto publisher = node->create_publisher("/topic", 10); EXPECT_EQ(publisher->get_subscription_count(), 0u); @@ -83,13 +85,10 @@ TEST_P(TestPublisherSubscriptionCount, increasing_and_decreasing_counts) rclcpp::sleep_for(offset); EXPECT_EQ(publisher->get_subscription_count(), 1u); EXPECT_EQ( - publisher->get_intra_process_subscription_count(), - parameters.intraprocess_count_results[0]); + publisher->get_intra_process_subscription_count(), parameters.intraprocess_count_results[0]); { - rclcpp::Node::SharedPtr another_node = std::make_shared( - "another_node", - "/ns", - parameters.node_options[1]); + rclcpp::Node::SharedPtr another_node = + std::make_shared("another_node", "/ns", parameters.node_options[1]); auto another_sub = another_node->create_subscription("/topic", 10, &OnMessage); @@ -102,8 +101,7 @@ TEST_P(TestPublisherSubscriptionCount, increasing_and_decreasing_counts) rclcpp::sleep_for(offset); EXPECT_EQ(publisher->get_subscription_count(), 1u); EXPECT_EQ( - publisher->get_intra_process_subscription_count(), - parameters.intraprocess_count_results[0]); + publisher->get_intra_process_subscription_count(), parameters.intraprocess_count_results[0]); } /** * Counts should be zero here, as all are subscriptions are out of scope. @@ -126,53 +124,37 @@ TestParameters parameters[] = { Testing publisher subscription count api and internal process subscription count. Two subscriptions in the same topic, both using intraprocess comm. */ - { - { - rclcpp::NodeOptions().use_intra_process_comms(true), - rclcpp::NodeOptions().use_intra_process_comms(true) - }, - {1u, 2u}, - "two_subscriptions_intraprocess_comm" - }, + {{rclcpp::NodeOptions().use_intra_process_comms(true), + rclcpp::NodeOptions().use_intra_process_comms(true)}, + {1u, 2u}, + "two_subscriptions_intraprocess_comm"}, /* Testing publisher subscription count api and internal process subscription count. Two subscriptions, one using intra-process comm and the other not using it. */ - { - { - rclcpp::NodeOptions().use_intra_process_comms(true), - rclcpp::NodeOptions().use_intra_process_comms(false) - }, - {1u, 1u}, - "two_subscriptions_one_intraprocess_one_not" - }, + {{rclcpp::NodeOptions().use_intra_process_comms(true), + rclcpp::NodeOptions().use_intra_process_comms(false)}, + {1u, 1u}, + "two_subscriptions_one_intraprocess_one_not"}, /* Testing publisher subscription count api and internal process subscription count. Two contexts, both using intra-process. */ - { - { - rclcpp::NodeOptions().use_intra_process_comms(true), - rclcpp::NodeOptions().context(get_new_context()).use_intra_process_comms(true) - }, - {1u, 1u}, - "two_subscriptions_in_two_contexts_with_intraprocess_comm" - }, + {{rclcpp::NodeOptions().use_intra_process_comms(true), + rclcpp::NodeOptions().context(get_new_context()).use_intra_process_comms(true)}, + {1u, 1u}, + "two_subscriptions_in_two_contexts_with_intraprocess_comm"}, /* Testing publisher subscription count api and internal process subscription count. Two contexts, both of them not using intra-process comm. */ - { - { - rclcpp::NodeOptions().use_intra_process_comms(false), - rclcpp::NodeOptions().context(get_new_context()).use_intra_process_comms(false) - }, - {0u, 0u}, - "two_subscriptions_in_two_contexts_without_intraprocess_comm" - } -}; + {{rclcpp::NodeOptions().use_intra_process_comms(false), + rclcpp::NodeOptions().context(get_new_context()).use_intra_process_comms(false)}, + {0u, 0u}, + "two_subscriptions_in_two_contexts_without_intraprocess_comm"}}; INSTANTIATE_TEST_CASE_P( - TestWithDifferentNodeOptions, TestPublisherSubscriptionCount, + TestWithDifferentNodeOptions, + TestPublisherSubscriptionCount, ::testing::ValuesIn(parameters), ::testing::PrintToStringParamName()); diff --git a/rclcpp/test/test_rate.cpp b/rclcpp/test/test_rate.cpp index a4c1c4386a..618dad9e94 100644 --- a/rclcpp/test/test_rate.cpp +++ b/rclcpp/test/test_rate.cpp @@ -21,7 +21,8 @@ /* Basic tests for the Rate and WallRate classes. */ -TEST(TestRate, rate_basics) { +TEST(TestRate, rate_basics) +{ auto period = std::chrono::milliseconds(100); auto offset = std::chrono::milliseconds(50); auto epsilon = std::chrono::milliseconds(1); @@ -60,7 +61,8 @@ TEST(TestRate, rate_basics) { ASSERT_TRUE(epsilon > delta); } -TEST(TestRate, wall_rate_basics) { +TEST(TestRate, wall_rate_basics) +{ auto period = std::chrono::milliseconds(100); auto offset = std::chrono::milliseconds(50); auto epsilon = std::chrono::milliseconds(1); diff --git a/rclcpp/test/test_serialized_message_allocator.cpp b/rclcpp/test/test_serialized_message_allocator.cpp index bc177722e4..7f585a6c5c 100644 --- a/rclcpp/test/test_serialized_message_allocator.cpp +++ b/rclcpp/test/test_serialized_message_allocator.cpp @@ -14,8 +14,8 @@ #include -#include #include +#include #include "rclcpp/rclcpp.hpp" @@ -23,7 +23,8 @@ #include "test_msgs/msg/empty.hpp" -TEST(TestSerializedMessageAllocator, default_allocator) { +TEST(TestSerializedMessageAllocator, default_allocator) +{ using DummyMessageT = float; auto mem_strategy = rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); @@ -51,14 +52,13 @@ TEST(TestSerializedMessageAllocator, default_allocator) { mem_strategy->return_serialized_message(msg1000); } -TEST(TestSerializedMessageAllocator, borrow_from_subscription) { +TEST(TestSerializedMessageAllocator, borrow_from_subscription) +{ rclcpp::init(0, NULL); auto node = std::make_shared("test_serialized_message_allocator_node"); - std::shared_ptr sub = - node->create_subscription( - "~/dummy_topic", 10, - [](std::shared_ptr test_msg) {(void) test_msg;}); + std::shared_ptr sub = node->create_subscription( + "~/dummy_topic", 10, [](std::shared_ptr test_msg) { (void)test_msg; }); auto msg0 = sub->create_serialized_message(); EXPECT_EQ(0u, msg0->buffer_capacity); diff --git a/rclcpp/test/test_service.cpp b/rclcpp/test/test_service.cpp index 9ab90e1208..670de8ec36 100644 --- a/rclcpp/test/test_service.cpp +++ b/rclcpp/test/test_service.cpp @@ -14,8 +14,8 @@ #include -#include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" @@ -68,31 +68,30 @@ class TestServiceSub : public ::testing::Test /* Testing service construction and destruction. */ -TEST_F(TestService, construction_and_destruction) { +TEST_F(TestService, construction_and_destruction) +{ using rcl_interfaces::srv::ListParameters; auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { - }; + [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) {}; { auto service = node->create_service("service", callback); } { ASSERT_THROW( - { - auto service = node->create_service("invalid_service?", callback); - }, rclcpp::exceptions::InvalidServiceNameError); + { auto service = node->create_service("invalid_service?", callback); }, + rclcpp::exceptions::InvalidServiceNameError); } } /* Testing service construction and destruction for subnodes. */ -TEST_F(TestServiceSub, construction_and_destruction) { +TEST_F(TestServiceSub, construction_and_destruction) +{ using rcl_interfaces::srv::ListParameters; auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { - }; + [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) {}; { auto service = subnode->create_service("service", callback); EXPECT_STREQ(service->get_service_name(), "/ns/sub_ns/service"); @@ -100,8 +99,7 @@ TEST_F(TestServiceSub, construction_and_destruction) { { ASSERT_THROW( - { - auto service = node->create_service("invalid_service?", callback); - }, rclcpp::exceptions::InvalidServiceNameError); + { auto service = node->create_service("invalid_service?", callback); }, + rclcpp::exceptions::InvalidServiceNameError); } } diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp index f3a0db68ed..d306d3b1d6 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -14,8 +14,8 @@ #include -#include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" @@ -80,8 +80,7 @@ class TestSubscriptionSub : public ::testing::Test class SubscriptionClassNodeInheritance : public rclcpp::Node { public: - SubscriptionClassNodeInheritance() - : Node("subscription_class_node_inheritance") + SubscriptionClassNodeInheritance() : Node("subscription_class_node_inheritance") { } @@ -92,8 +91,8 @@ class SubscriptionClassNodeInheritance : public rclcpp::Node void CreateSubscription() { - auto callback = std::bind( - &SubscriptionClassNodeInheritance::OnMessage, this, std::placeholders::_1); + auto callback = + std::bind(&SubscriptionClassNodeInheritance::OnMessage, this, std::placeholders::_1); using rcl_interfaces::msg::IntraProcessMessage; auto sub = this->create_subscription("topic", 10, callback); } @@ -119,31 +118,30 @@ class SubscriptionClass /* Testing subscription construction and destruction. */ -TEST_F(TestSubscription, construction_and_destruction) { +TEST_F(TestSubscription, construction_and_destruction) +{ using rcl_interfaces::msg::IntraProcessMessage; - auto callback = [](const IntraProcessMessage::SharedPtr msg) { - (void)msg; - }; + auto callback = [](const IntraProcessMessage::SharedPtr msg) { (void)msg; }; { auto sub = node->create_subscription("topic", 10, callback); } { ASSERT_THROW( - { - auto sub = node->create_subscription("invalid_topic?", 10, callback); - }, rclcpp::exceptions::InvalidTopicNameError); + { + auto sub = node->create_subscription("invalid_topic?", 10, callback); + }, + rclcpp::exceptions::InvalidTopicNameError); } } /* Testing subscription construction and destruction for subnodes. */ -TEST_F(TestSubscriptionSub, construction_and_destruction) { +TEST_F(TestSubscriptionSub, construction_and_destruction) +{ using rcl_interfaces::msg::IntraProcessMessage; - auto callback = [](const IntraProcessMessage::SharedPtr msg) { - (void)msg; - }; + auto callback = [](const IntraProcessMessage::SharedPtr msg) { (void)msg; }; { auto sub = subnode->create_subscription("topic", 1, callback); EXPECT_STREQ(sub->get_topic_name(), "/ns/sub_ns/topic"); @@ -163,16 +161,16 @@ TEST_F(TestSubscriptionSub, construction_and_destruction) { { ASSERT_THROW( - { - auto sub = node->create_subscription("invalid_topic?", 1, callback); - }, rclcpp::exceptions::InvalidTopicNameError); + { auto sub = node->create_subscription("invalid_topic?", 1, callback); }, + rclcpp::exceptions::InvalidTopicNameError); } } /* Testing subscription creation signatures. */ -TEST_F(TestSubscription, various_creation_signatures) { +TEST_F(TestSubscription, various_creation_signatures) +{ using rcl_interfaces::msg::IntraProcessMessage; auto cb = [](rcl_interfaces::msg::IntraProcessMessage::SharedPtr) {}; { @@ -208,7 +206,8 @@ TEST_F(TestSubscription, various_creation_signatures) { /* Testing subscriptions using std::bind. */ -TEST_F(TestSubscription, callback_bind) { +TEST_F(TestSubscription, callback_bind) +{ using rcl_interfaces::msg::IntraProcessMessage; { // Member callback for plain class diff --git a/rclcpp/test/test_subscription_publisher_count_api.cpp b/rclcpp/test/test_subscription_publisher_count_api.cpp index cdf1bf95d5..2b200912f4 100644 --- a/rclcpp/test/test_subscription_publisher_count_api.cpp +++ b/rclcpp/test/test_subscription_publisher_count_api.cpp @@ -15,8 +15,8 @@ #include #include -#include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" @@ -46,8 +46,12 @@ class TestSubscriptionPublisherCount : public ::testing::TestWithParam( - "my_node", - "/ns", - node_options); + rclcpp::Node::SharedPtr node = std::make_shared("my_node", "/ns", node_options); auto subscription = node->create_subscription("/topic", 10, &OnMessage); EXPECT_EQ(subscription->get_publisher_count(), 0u); @@ -73,12 +74,9 @@ TEST_P(TestSubscriptionPublisherCount, increasing_and_decreasing_counts) rclcpp::sleep_for(offset); EXPECT_EQ(subscription->get_publisher_count(), 1u); { - rclcpp::Node::SharedPtr another_node = std::make_shared( - "another_node", - "/ns", - node_options); - auto another_pub = - another_node->create_publisher("/topic", 10); + rclcpp::Node::SharedPtr another_node = + std::make_shared("another_node", "/ns", node_options); + auto another_pub = another_node->create_publisher("/topic", 10); rclcpp::sleep_for(offset); EXPECT_EQ(subscription->get_publisher_count(), 2u); @@ -102,19 +100,12 @@ TestParameters parameters[] = { Testing subscription publisher count api. One context. */ - { - rclcpp::NodeOptions(), - "one_context_test" - }, + {rclcpp::NodeOptions(), "one_context_test"}, /* Testing subscription publisher count api. Two contexts. */ - { - rclcpp::NodeOptions().context(get_new_context()), - "two_contexts_test" - } -}; + {rclcpp::NodeOptions().context(get_new_context()), "two_contexts_test"}}; INSTANTIATE_TEST_CASE_P( TestWithDifferentNodeOptions, diff --git a/rclcpp/test/test_subscription_traits.cpp b/rclcpp/test/test_subscription_traits.cpp index 3112cafffd..9f30f77f38 100644 --- a/rclcpp/test/test_subscription_traits.cpp +++ b/rclcpp/test/test_subscription_traits.cpp @@ -27,32 +27,33 @@ void serialized_callback_copy(rcl_serialized_message_t unused) { - (void) unused; + (void)unused; } void serialized_callback_shared_ptr(std::shared_ptr unused) { - (void) unused; + (void)unused; } void not_serialized_callback(char * unused) { - (void) unused; + (void)unused; } void not_serialized_shared_ptr_callback(std::shared_ptr unused) { - (void) unused; + (void)unused; } void not_serialized_unique_ptr_callback( - test_msgs::msg::Empty::UniquePtrWithDeleter, - test_msgs::msg::Empty>> unused) + test_msgs::msg::Empty::UniquePtrWithDeleter< + rclcpp::allocator::Deleter, test_msgs::msg::Empty>> unused) { - (void) unused; + (void)unused; } -TEST(TestSubscriptionTraits, is_serialized_callback) { +TEST(TestSubscriptionTraits, is_serialized_callback) +{ // Test regular functions auto cb1 = &serialized_callback_copy; static_assert( @@ -74,10 +75,7 @@ TEST(TestSubscriptionTraits, is_serialized_callback) { rclcpp::subscription_traits::is_serialized_callback::value == false, "passing a std::shared_tr is not a serialized callback"); - auto cb5 = [](rcl_serialized_message_t unused) -> void - { - (void) unused; - }; + auto cb5 = [](rcl_serialized_message_t unused) -> void { (void)unused; }; static_assert( rclcpp::subscription_traits::is_serialized_callback::value == true, "rcl_serialized_message_t in a first argument callback makes it a serialized callback"); @@ -85,10 +83,9 @@ TEST(TestSubscriptionTraits, is_serialized_callback) { using MessageT = test_msgs::msg::Empty; using MessageTAllocator = std::allocator; using MessageTDeallocator = rclcpp::allocator::Deleter; - auto cb6 = [](MessageT::UniquePtrWithDeleter unique_msg_ptr) -> void - { - (void) unique_msg_ptr; - }; + auto cb6 = [](MessageT::UniquePtrWithDeleter unique_msg_ptr) -> void { + (void)unique_msg_ptr; + }; static_assert( rclcpp::subscription_traits::is_serialized_callback::value == false, "passing a std::unique_ptr of test_msgs::msg::Empty is not a serialized callback"); @@ -99,24 +96,21 @@ TEST(TestSubscriptionTraits, is_serialized_callback) { "passing a fancy unique_ptr of test_msgs::msg::Empty is not a serialized callback"); } -TEST(TestSubscriptionTraits, callback_messages) { +TEST(TestSubscriptionTraits, callback_messages) +{ static_assert( std::is_same< std::shared_ptr, - rclcpp::function_traits::function_traits< - decltype(not_serialized_shared_ptr_callback) - >::template argument_type<0> - >::value, "wrong!"); + rclcpp::function_traits::function_traits::template argument_type<0>>::value, + "wrong!"); static_assert( std::is_same< char, - rclcpp::subscription_traits::extract_message_type< - rclcpp::function_traits::function_traits< - decltype(not_serialized_shared_ptr_callback) - >::template argument_type<0> - >::type - >::value, "wrong!"); + rclcpp::subscription_traits::extract_message_type::template argument_type<0>>::type>::value, + "wrong!"); auto cb1 = &serialized_callback_copy; static_assert( @@ -134,22 +128,15 @@ TEST(TestSubscriptionTraits, callback_messages) { auto cb3 = ¬_serialized_callback; static_assert( - std::is_same< - char *, - rclcpp::subscription_traits::has_message_type::type>::value, + std::is_same::type>::value, "not serialized callback message type is char"); auto cb4 = ¬_serialized_shared_ptr_callback; static_assert( - std::is_same< - char, - rclcpp::subscription_traits::has_message_type::type>::value, + std::is_same::type>::value, "not serialized shared_ptr callback message type is std::shared_ptr"); - auto cb5 = [](rcl_serialized_message_t unused) -> void - { - (void) unused; - }; + auto cb5 = [](rcl_serialized_message_t unused) -> void { (void)unused; }; static_assert( std::is_same< rcl_serialized_message_t, @@ -159,10 +146,9 @@ TEST(TestSubscriptionTraits, callback_messages) { using MessageT = test_msgs::msg::Empty; using MessageTAllocator = std::allocator; using MessageTDeallocator = rclcpp::allocator::Deleter; - auto cb6 = [](std::unique_ptr unique_msg_ptr) -> void - { - (void) unique_msg_ptr; - }; + auto cb6 = [](std::unique_ptr unique_msg_ptr) -> void { + (void)unique_msg_ptr; + }; static_assert( std::is_same< test_msgs::msg::Empty, diff --git a/rclcpp/test/test_time.cpp b/rclcpp/test/test_time.cpp index 22c9772647..85b50e0df2 100644 --- a/rclcpp/test/test_time.cpp +++ b/rclcpp/test/test_time.cpp @@ -35,7 +35,6 @@ bool logical_eq(const bool a, const bool b) } // namespace - class TestTime : public ::testing::Test { protected: @@ -45,7 +44,8 @@ class TestTime : public ::testing::Test } }; -TEST(TestTime, clock_type_access) { +TEST(TestTime, clock_type_access) +{ rclcpp::Clock ros_clock(RCL_ROS_TIME); EXPECT_EQ(RCL_ROS_TIME, ros_clock.get_clock_type()); @@ -56,7 +56,8 @@ TEST(TestTime, clock_type_access) { EXPECT_EQ(RCL_STEADY_TIME, steady_clock.get_clock_type()); } -TEST(TestTime, time_sources) { +TEST(TestTime, time_sources) +{ using builtin_interfaces::msg::Time; rclcpp::Clock ros_clock(RCL_ROS_TIME); Time ros_now = ros_clock.now(); @@ -74,7 +75,8 @@ TEST(TestTime, time_sources) { EXPECT_NE(0u, steady_now.nanosec); } -TEST(TestTime, conversions) { +TEST(TestTime, conversions) +{ rclcpp::Clock system_clock(RCL_SYSTEM_TIME); rclcpp::Time now = system_clock.now(); @@ -97,21 +99,18 @@ TEST(TestTime, conversions) { negative_time_msg.sec = -1; negative_time_msg.nanosec = 1; - EXPECT_ANY_THROW( - { - rclcpp::Time negative_time = negative_time_msg; - }); + EXPECT_ANY_THROW({ rclcpp::Time negative_time = negative_time_msg; }); EXPECT_ANY_THROW(rclcpp::Time(-1, 1)); - EXPECT_ANY_THROW( - { + EXPECT_ANY_THROW({ rclcpp::Time assignment(1, 2); assignment = negative_time_msg; }); } -TEST(TestTime, operators) { +TEST(TestTime, operators) +{ rclcpp::Time old(1, 0); rclcpp::Time young(2, 0); @@ -162,7 +161,8 @@ TEST(TestTime, operators) { } } -TEST(TestTime, overflow_detectors) { +TEST(TestTime, overflow_detectors) +{ ///////////////////////////////////////////////////////////////////////////// // Test logical_eq call first: EXPECT_TRUE(logical_eq(false, false)); @@ -185,23 +185,19 @@ TEST(TestTime, overflow_detectors) { const big_type_t sum = x + y; const big_type_t diff = x - y; - const bool add_will_overflow = - rclcpp::add_will_overflow(test_type_t(x), test_type_t(y)); + const bool add_will_overflow = rclcpp::add_will_overflow(test_type_t(x), test_type_t(y)); const bool add_did_overflow = sum > max_val; EXPECT_TRUE(logical_eq(add_will_overflow, add_did_overflow)); - const bool add_will_underflow = - rclcpp::add_will_underflow(test_type_t(x), test_type_t(y)); + const bool add_will_underflow = rclcpp::add_will_underflow(test_type_t(x), test_type_t(y)); const bool add_did_underflow = sum < min_val; EXPECT_TRUE(logical_eq(add_will_underflow, add_did_underflow)); - const bool sub_will_overflow = - rclcpp::sub_will_overflow(test_type_t(x), test_type_t(y)); + const bool sub_will_overflow = rclcpp::sub_will_overflow(test_type_t(x), test_type_t(y)); const bool sub_did_overflow = diff > max_val; EXPECT_TRUE(logical_eq(sub_will_overflow, sub_did_overflow)); - const bool sub_will_underflow = - rclcpp::sub_will_underflow(test_type_t(x), test_type_t(y)); + const bool sub_will_underflow = rclcpp::sub_will_underflow(test_type_t(x), test_type_t(y)); const bool sub_did_underflow = diff < min_val; EXPECT_TRUE(logical_eq(sub_will_underflow, sub_did_underflow)); } @@ -219,7 +215,8 @@ TEST(TestTime, overflow_detectors) { EXPECT_TRUE(rclcpp::sub_will_underflow(INT64_MIN, 1)); } -TEST(TestTime, overflows) { +TEST(TestTime, overflows) +{ rclcpp::Time max_time(std::numeric_limits::max()); rclcpp::Time min_time(std::numeric_limits::min()); rclcpp::Duration one(1); @@ -247,7 +244,8 @@ TEST(TestTime, overflows) { EXPECT_NO_THROW(one_time - two_time); } -TEST(TestTime, seconds) { +TEST(TestTime, seconds) +{ EXPECT_DOUBLE_EQ(0.0, rclcpp::Time(0, 0).seconds()); EXPECT_DOUBLE_EQ(4.5, rclcpp::Time(4, 500000000).seconds()); EXPECT_DOUBLE_EQ(2.5, rclcpp::Time(0, 2500000000).seconds()); diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index bd8a1868a8..2bcf588d84 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/rclcpp/test/test_time_source.cpp @@ -92,9 +92,7 @@ void spin_until_time( } void spin_until_ros_time_updated( - rclcpp::Clock::SharedPtr clock, - rclcpp::Node::SharedPtr node, - rclcpp::ParameterValue value) + rclcpp::Clock::SharedPtr clock, rclcpp::Node::SharedPtr node, rclcpp::ParameterValue value) { // Similar to above: Call spin_once until we see the clock's ros_time_is_active method // match the ParameterValue @@ -144,27 +142,19 @@ void trigger_clock_changes( // to a clock change callback and spin until future complete, but that's an upstream // change spin_until_time( - clock, - node, - std::chrono::seconds(i) + std::chrono::nanoseconds(1000), - expect_time_update - ); + clock, node, std::chrono::seconds(i) + std::chrono::nanoseconds(1000), expect_time_update); } } void set_use_sim_time_parameter( - rclcpp::Node::SharedPtr node, - rclcpp::ParameterValue value, - rclcpp::Clock::SharedPtr clock) + rclcpp::Node::SharedPtr node, rclcpp::ParameterValue value, rclcpp::Clock::SharedPtr clock) { auto parameters_client = std::make_shared(node); using namespace std::chrono_literals; EXPECT_TRUE(parameters_client->wait_for_service(2s)); - auto set_parameters_results = parameters_client->set_parameters( - { - rclcpp::Parameter("use_sim_time", value) - }); + auto set_parameters_results = + parameters_client->set_parameters({rclcpp::Parameter("use_sim_time", value)}); for (auto & result : set_parameters_results) { EXPECT_TRUE(result.successful); } @@ -176,7 +166,8 @@ void set_use_sim_time_parameter( spin_until_ros_time_updated(clock, node, value); } -TEST_F(TestTimeSource, detachUnattached) { +TEST_F(TestTimeSource, detachUnattached) +{ rclcpp::TimeSource ts; ASSERT_NO_THROW(ts.detachNode()); @@ -185,14 +176,16 @@ TEST_F(TestTimeSource, detachUnattached) { ASSERT_NO_THROW(ts.detachNode()); } -TEST_F(TestTimeSource, reattach) { +TEST_F(TestTimeSource, reattach) +{ rclcpp::TimeSource ts; // Try reattach ASSERT_NO_THROW(ts.attachNode(node)); ASSERT_NO_THROW(ts.attachNode(node)); } -TEST_F(TestTimeSource, ROS_time_valid_attach_detach) { +TEST_F(TestTimeSource, ROS_time_valid_attach_detach) +{ rclcpp::TimeSource ts; auto ros_clock = std::make_shared(RCL_ROS_TIME); @@ -211,7 +204,8 @@ TEST_F(TestTimeSource, ROS_time_valid_attach_detach) { EXPECT_FALSE(ros_clock->ros_time_is_active()); } -TEST_F(TestTimeSource, ROS_time_valid_wall_time) { +TEST_F(TestTimeSource, ROS_time_valid_wall_time) +{ rclcpp::TimeSource ts; auto ros_clock = std::make_shared(RCL_ROS_TIME); auto ros_clock2 = std::make_shared(RCL_ROS_TIME); @@ -226,7 +220,8 @@ TEST_F(TestTimeSource, ROS_time_valid_wall_time) { EXPECT_FALSE(ros_clock2->ros_time_is_active()); } -TEST_F(TestTimeSource, ROS_time_valid_sim_time) { +TEST_F(TestTimeSource, ROS_time_valid_sim_time) +{ rclcpp::TimeSource ts; auto ros_clock = std::make_shared(RCL_ROS_TIME); auto ros_clock2 = std::make_shared(RCL_ROS_TIME); @@ -242,7 +237,8 @@ TEST_F(TestTimeSource, ROS_time_valid_sim_time) { EXPECT_TRUE(ros_clock2->ros_time_is_active()); } -TEST_F(TestTimeSource, clock) { +TEST_F(TestTimeSource, clock) +{ rclcpp::TimeSource ts(node); auto ros_clock = std::make_shared(RCL_ROS_TIME); EXPECT_FALSE(ros_clock->ros_time_is_active()); @@ -287,12 +283,14 @@ class CallbackObject rcl_time_jump_t last_timejump_; void post_callback(const rcl_time_jump_t & jump, int id) { - last_postcallback_id_ = id; last_timejump_ = jump; + last_postcallback_id_ = id; + last_timejump_ = jump; ++post_callback_calls_; } }; -TEST_F(TestTimeSource, callbacks) { +TEST_F(TestTimeSource, callbacks) +{ CallbackObject cbo; rcl_jump_threshold_t jump_threshold; jump_threshold.min_forward.nanoseconds = 0; @@ -383,8 +381,8 @@ TEST_F(TestTimeSource, callbacks) { EXPECT_EQ(4, cbo.last_postcallback_id_); } - -TEST_F(TestTimeSource, callback_handler_erasure) { +TEST_F(TestTimeSource, callback_handler_erasure) +{ CallbackObject cbo; rcl_jump_threshold_t jump_threshold; jump_threshold.min_forward.nanoseconds = 0; @@ -454,7 +452,8 @@ TEST_F(TestTimeSource, callback_handler_erasure) { EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); } -TEST_F(TestTimeSource, parameter_activation) { +TEST_F(TestTimeSource, parameter_activation) +{ rclcpp::TimeSource ts(node); auto ros_clock = std::make_shared(RCL_ROS_TIME); EXPECT_FALSE(ros_clock->ros_time_is_active()); @@ -478,7 +477,8 @@ TEST_F(TestTimeSource, parameter_activation) { EXPECT_TRUE(ros_clock->ros_time_is_active()); } -TEST_F(TestTimeSource, no_pre_jump_callback) { +TEST_F(TestTimeSource, no_pre_jump_callback) +{ CallbackObject cbo; rcl_jump_threshold_t jump_threshold; jump_threshold.min_forward.nanoseconds = 0; diff --git a/rclcpp/test/test_timer.cpp b/rclcpp/test/test_timer.cpp index 2d9022e045..d3855d195b 100644 --- a/rclcpp/test/test_timer.cpp +++ b/rclcpp/test/test_timer.cpp @@ -40,19 +40,15 @@ class TestTimer : public ::testing::Test test_node = std::make_shared("test_timer_node"); - timer = test_node->create_wall_timer( - 100ms, - [this]() -> void - { - this->has_timer_run.store(true); - - if (this->cancel_timer.load()) { - this->timer->cancel(); - } - // prevent any tests running timer from blocking - this->executor->cancel(); + timer = test_node->create_wall_timer(100ms, [this]() -> void { + this->has_timer_run.store(true); + + if (this->cancel_timer.load()) { + this->timer->cancel(); } - ); + // prevent any tests running timer from blocking + this->executor->cancel(); + }); executor->add_node(test_node); // don't start spinning, let the test dictate when @@ -78,8 +74,7 @@ class TestTimer : public ::testing::Test /// check if initial states are set as expected void test_initial_conditions( - std::shared_ptr & timer, - std::atomic & has_timer_run) + std::shared_ptr & timer, std::atomic & has_timer_run) { ASSERT_FALSE(timer->is_canceled()); ASSERT_FALSE(has_timer_run.load()); diff --git a/rclcpp/test/test_utilities.cpp b/rclcpp/test/test_utilities.cpp index dc0a301976..5d46755937 100644 --- a/rclcpp/test/test_utilities.cpp +++ b/rclcpp/test/test_utilities.cpp @@ -14,21 +14,25 @@ #include -#include #include +#include #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/utilities.hpp" -TEST(TestUtilities, remove_ros_arguments) { - const char * const argv[] = { - "process_name", - "-d", "--ros-args", - "-r", "__ns:=/foo/bar", - "-r", "__ns:=/fiz/buz", - "--", "--foo=bar", "--baz" - }; +TEST(TestUtilities, remove_ros_arguments) +{ + const char * const argv[] = {"process_name", + "-d", + "--ros-args", + "-r", + "__ns:=/foo/bar", + "-r", + "__ns:=/fiz/buz", + "--", + "--foo=bar", + "--baz"}; int argc = sizeof(argv) / sizeof(const char *); auto args = rclcpp::remove_ros_arguments(argc, argv); @@ -39,16 +43,15 @@ TEST(TestUtilities, remove_ros_arguments) { ASSERT_EQ(std::string{"--baz"}, args[3]); } -TEST(TestUtilities, remove_ros_arguments_null) { +TEST(TestUtilities, remove_ros_arguments_null) +{ // In the case of a C executable, we would expect to get // argc=1 and argv = ["process_name"], so this is an invalid input. - ASSERT_THROW( - { - rclcpp::remove_ros_arguments(0, nullptr); - }, rclcpp::exceptions::RCLErrorBase); + ASSERT_THROW({ rclcpp::remove_ros_arguments(0, nullptr); }, rclcpp::exceptions::RCLErrorBase); } -TEST(TestUtilities, init_with_args) { +TEST(TestUtilities, init_with_args) +{ const char * const argv[] = {"process_name"}; int argc = sizeof(argv) / sizeof(const char *); auto other_args = rclcpp::init_and_remove_ros_arguments(argc, argv); @@ -60,7 +63,8 @@ TEST(TestUtilities, init_with_args) { rclcpp::shutdown(); } -TEST(TestUtilities, multi_init) { +TEST(TestUtilities, multi_init) +{ auto context1 = std::make_shared(); auto context2 = std::make_shared(); diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index d94508feab..2175ad4646 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -15,11 +15,11 @@ #ifndef RCLCPP_ACTION__CLIENT_HPP_ #define RCLCPP_ACTION__CLIENT_HPP_ +#include #include #include -#include #include -#include +#include #include #include @@ -40,7 +40,6 @@ #include "rclcpp_action/types.hpp" #include "rclcpp_action/visibility_control.hpp" - namespace rclcpp_action { // Forward declaration @@ -62,18 +61,15 @@ class ClientBase : public rclcpp::Waitable /// Return true if there is an action server that is ready to take goal requests. RCLCPP_ACTION_PUBLIC - bool - action_server_is_ready() const; + bool action_server_is_ready() const; /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. template - bool - wait_for_action_server( + bool wait_for_action_server( std::chrono::duration timeout = std::chrono::duration(-1)) { return wait_for_action_server_nanoseconds( - std::chrono::duration_cast(timeout) - ); + std::chrono::duration_cast(timeout)); } // ------------- @@ -81,43 +77,35 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC - size_t - get_number_of_ready_subscriptions() override; + size_t get_number_of_ready_subscriptions() override; /// \internal RCLCPP_ACTION_PUBLIC - size_t - get_number_of_ready_timers() override; + size_t get_number_of_ready_timers() override; /// \internal RCLCPP_ACTION_PUBLIC - size_t - get_number_of_ready_clients() override; + size_t get_number_of_ready_clients() override; /// \internal RCLCPP_ACTION_PUBLIC - size_t - get_number_of_ready_services() override; + size_t get_number_of_ready_services() override; /// \internal RCLCPP_ACTION_PUBLIC - size_t - get_number_of_ready_guard_conditions() override; + size_t get_number_of_ready_guard_conditions() override; /// \internal RCLCPP_ACTION_PUBLIC - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; + bool add_to_wait_set(rcl_wait_set_t * wait_set) override; /// \internal RCLCPP_ACTION_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; + bool is_ready(rcl_wait_set_t * wait_set) override; /// \internal RCLCPP_ACTION_PUBLIC - void - execute() override; + void execute() override; // End Waitables API // ----------------- @@ -134,12 +122,11 @@ class ClientBase : public rclcpp::Waitable /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. RCLCPP_ACTION_PUBLIC - bool - wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout); + bool wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout); // ----------------------------------------------------- // API for communication between ClientBase and Client<> - using ResponseCallback = std::function response)>; + using ResponseCallback = std::function response)>; /// \internal RCLCPP_ACTION_PUBLIC @@ -147,92 +134,55 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC - virtual - GoalUUID - generate_goal_id(); + virtual GoalUUID generate_goal_id(); /// \internal RCLCPP_ACTION_PUBLIC - virtual - void - send_goal_request( - std::shared_ptr request, - ResponseCallback callback); + virtual void send_goal_request(std::shared_ptr request, ResponseCallback callback); /// \internal RCLCPP_ACTION_PUBLIC - virtual - void - send_result_request( - std::shared_ptr request, - ResponseCallback callback); + virtual void send_result_request(std::shared_ptr request, ResponseCallback callback); /// \internal RCLCPP_ACTION_PUBLIC - virtual - void - send_cancel_request( - std::shared_ptr request, - ResponseCallback callback); + virtual void send_cancel_request(std::shared_ptr request, ResponseCallback callback); /// \internal - virtual - std::shared_ptr - create_goal_response() const = 0; + virtual std::shared_ptr create_goal_response() const = 0; /// \internal RCLCPP_ACTION_PUBLIC - virtual - void - handle_goal_response( - const rmw_request_id_t & response_header, - std::shared_ptr goal_response); + virtual void handle_goal_response( + const rmw_request_id_t & response_header, std::shared_ptr goal_response); /// \internal - virtual - std::shared_ptr - create_result_response() const = 0; + virtual std::shared_ptr create_result_response() const = 0; /// \internal RCLCPP_ACTION_PUBLIC - virtual - void - handle_result_response( - const rmw_request_id_t & response_header, - std::shared_ptr result_response); + virtual void handle_result_response( + const rmw_request_id_t & response_header, std::shared_ptr result_response); /// \internal - virtual - std::shared_ptr - create_cancel_response() const = 0; + virtual std::shared_ptr create_cancel_response() const = 0; /// \internal RCLCPP_ACTION_PUBLIC - virtual - void - handle_cancel_response( - const rmw_request_id_t & response_header, - std::shared_ptr cancel_response); + virtual void handle_cancel_response( + const rmw_request_id_t & response_header, std::shared_ptr cancel_response); /// \internal - virtual - std::shared_ptr - create_feedback_message() const = 0; + virtual std::shared_ptr create_feedback_message() const = 0; /// \internal - virtual - void - handle_feedback_message(std::shared_ptr message) = 0; + virtual void handle_feedback_message(std::shared_ptr message) = 0; /// \internal - virtual - std::shared_ptr - create_status_message() const = 0; + virtual std::shared_ptr create_status_message() const = 0; /// \internal - virtual - void - handle_status_message(std::shared_ptr message) = 0; + virtual void handle_status_message(std::shared_ptr message) = 0; // End API for communication between ClientBase and Client<> // --------------------------------------------------------- @@ -262,12 +212,12 @@ class Client : public ClientBase using GoalHandle = ClientGoalHandle; using WrappedResult = typename GoalHandle::WrappedResult; using GoalResponseCallback = - std::function)>; + std::function)>; using FeedbackCallback = typename GoalHandle::FeedbackCallback; using ResultCallback = typename GoalHandle::ResultCallback; using CancelRequest = typename ActionT::Impl::CancelGoalService::Request; using CancelResponse = typename ActionT::Impl::CancelGoalService::Response; - using CancelCallback = std::function; + using CancelCallback = std::function; /// Options for sending a goal. /** @@ -276,9 +226,7 @@ class Client : public ClientBase struct SendGoalOptions { SendGoalOptions() - : goal_response_callback(nullptr), - feedback_callback(nullptr), - result_callback(nullptr) + : goal_response_callback(nullptr), feedback_callback(nullptr), result_callback(nullptr) { } @@ -317,10 +265,12 @@ class Client : public ClientBase rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, - const rcl_action_client_options_t client_options = rcl_action_client_get_default_options() - ) + const rcl_action_client_options_t client_options = rcl_action_client_get_default_options()) : ClientBase( - node_base, node_graph, node_logging, action_name, + node_base, + node_graph, + node_logging, + action_name, rosidl_typesupport_cpp::get_action_type_support_handle(), client_options) { @@ -339,8 +289,8 @@ class Client : public ClientBase * \return A future that completes when the goal has been accepted or rejected. * If the goal is rejected, then the result will be a `nullptr`. */ - std::shared_future - async_send_goal(const Goal & goal, const SendGoalOptions & options = SendGoalOptions()) + std::shared_future async_send_goal( + const Goal & goal, const SendGoalOptions & options = SendGoalOptions()) { // Put promise in the heap to move it around. auto promise = std::make_shared>(); @@ -351,8 +301,7 @@ class Client : public ClientBase goal_request->goal = goal; this->send_goal_request( std::static_pointer_cast(goal_request), - [this, goal_request, options, promise, future](std::shared_ptr response) mutable - { + [this, goal_request, options, promise, future](std::shared_ptr response) mutable { using GoalResponse = typename ActionT::Impl::SendGoalService::Response; auto goal_response = std::static_pointer_cast(response); if (!goal_response->accepted) { @@ -397,10 +346,8 @@ class Client : public ClientBase * \param[in] result_callback Optional callback that is called when the result is received. * \return A future that is set to the goal result when the goal is finished. */ - std::shared_future - async_get_result( - typename GoalHandle::SharedPtr goal_handle, - ResultCallback result_callback = nullptr) + std::shared_future async_get_result( + typename GoalHandle::SharedPtr goal_handle, ResultCallback result_callback = nullptr) { std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { @@ -427,10 +374,8 @@ class Client : public ClientBase * * action_msgs/CancelGoal.srv. */ - std::shared_future - async_cancel_goal( - typename GoalHandle::SharedPtr goal_handle, - CancelCallback cancel_callback = nullptr) + std::shared_future async_cancel_goal( + typename GoalHandle::SharedPtr goal_handle, CancelCallback cancel_callback = nullptr) { std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { @@ -452,14 +397,15 @@ class Client : public ClientBase * * action_msgs/CancelGoal.srv. */ - std::shared_future - async_cancel_all_goals(CancelCallback cancel_callback = nullptr) + std::shared_future async_cancel_all_goals( + CancelCallback cancel_callback = nullptr) { auto cancel_request = std::make_shared(); // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); std::fill( cancel_request->goal_info.goal_id.uuid.begin(), - cancel_request->goal_info.goal_id.uuid.end(), 0u); + cancel_request->goal_info.goal_id.uuid.end(), + 0u); return async_cancel(cancel_request, cancel_callback); } @@ -474,22 +420,20 @@ class Client : public ClientBase * * action_msgs/CancelGoal.srv. */ - std::shared_future - async_cancel_goals_before( - const rclcpp::Time & stamp, - CancelCallback cancel_callback = nullptr) + std::shared_future async_cancel_goals_before( + const rclcpp::Time & stamp, CancelCallback cancel_callback = nullptr) { auto cancel_request = std::make_shared(); // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); std::fill( cancel_request->goal_info.goal_id.uuid.begin(), - cancel_request->goal_info.goal_id.uuid.end(), 0u); + cancel_request->goal_info.goal_id.uuid.end(), + 0u); cancel_request->goal_info.stamp = stamp; return async_cancel(cancel_request, cancel_callback); } - virtual - ~Client() + virtual ~Client() { std::lock_guard guard(goal_handles_mutex_); auto it = goal_handles_.begin(); @@ -501,39 +445,34 @@ class Client : public ClientBase private: /// \internal - std::shared_ptr - create_goal_response() const override + std::shared_ptr create_goal_response() const override { using GoalResponse = typename ActionT::Impl::SendGoalService::Response; return std::shared_ptr(new GoalResponse()); } /// \internal - std::shared_ptr - create_result_response() const override + std::shared_ptr create_result_response() const override { using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; return std::shared_ptr(new GoalResultResponse()); } /// \internal - std::shared_ptr - create_cancel_response() const override + std::shared_ptr create_cancel_response() const override { return std::shared_ptr(new CancelResponse()); } /// \internal - std::shared_ptr - create_feedback_message() const override + std::shared_ptr create_feedback_message() const override { using FeedbackMessage = typename ActionT::Impl::FeedbackMessage; return std::shared_ptr(new FeedbackMessage()); } /// \internal - void - handle_feedback_message(std::shared_ptr message) override + void handle_feedback_message(std::shared_ptr message) override { std::lock_guard guard(goal_handles_mutex_); using FeedbackMessage = typename ActionT::Impl::FeedbackMessage; @@ -541,9 +480,7 @@ class Client : public ClientBase std::static_pointer_cast(message); const GoalUUID & goal_id = feedback_message->goal_id.uuid; if (goal_handles_.count(goal_id) == 0) { - RCLCPP_DEBUG( - this->get_logger(), - "Received feedback for unknown goal. Ignoring..."); + RCLCPP_DEBUG(this->get_logger(), "Received feedback for unknown goal. Ignoring..."); return; } typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; @@ -553,16 +490,14 @@ class Client : public ClientBase } /// \internal - std::shared_ptr - create_status_message() const override + std::shared_ptr create_status_message() const override { using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage; return std::shared_ptr(new GoalStatusMessage()); } /// \internal - void - handle_status_message(std::shared_ptr message) override + void handle_status_message(std::shared_ptr message) override { std::lock_guard guard(goal_handles_mutex_); using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage; @@ -570,27 +505,22 @@ class Client : public ClientBase for (const GoalStatus & status : status_message->status_list) { const GoalUUID & goal_id = status.goal_info.goal_id.uuid; if (goal_handles_.count(goal_id) == 0) { - RCLCPP_DEBUG( - this->get_logger(), - "Received status for unknown goal. Ignoring..."); + RCLCPP_DEBUG(this->get_logger(), "Received status for unknown goal. Ignoring..."); continue; } typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; goal_handle->set_status(status.status); const int8_t goal_status = goal_handle->get_status(); if ( - goal_status == GoalStatus::STATUS_SUCCEEDED || - goal_status == GoalStatus::STATUS_CANCELED || - goal_status == GoalStatus::STATUS_ABORTED) - { + goal_status == GoalStatus::STATUS_SUCCEEDED || goal_status == GoalStatus::STATUS_CANCELED || + goal_status == GoalStatus::STATUS_ABORTED) { goal_handles_.erase(goal_id); } } } /// \internal - void - make_result_aware(typename GoalHandle::SharedPtr goal_handle) + void make_result_aware(typename GoalHandle::SharedPtr goal_handle) { // Avoid making more than one request if (goal_handle->set_result_awareness(true)) { @@ -601,8 +531,7 @@ class Client : public ClientBase goal_result_request->goal_id.uuid = goal_handle->get_goal_id(); this->send_result_request( std::static_pointer_cast(goal_result_request), - [goal_handle, this](std::shared_ptr response) mutable - { + [goal_handle, this](std::shared_ptr response) mutable { // Wrap the response in a struct with the fields a user cares about WrappedResult wrapped_result; using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; @@ -618,18 +547,15 @@ class Client : public ClientBase } /// \internal - std::shared_future - async_cancel( - typename CancelRequest::SharedPtr cancel_request, - CancelCallback cancel_callback = nullptr) + std::shared_future async_cancel( + typename CancelRequest::SharedPtr cancel_request, CancelCallback cancel_callback = nullptr) { // Put promise in the heap to move it around. auto promise = std::make_shared>(); std::shared_future future(promise->get_future()); this->send_cancel_request( std::static_pointer_cast(cancel_request), - [cancel_callback, promise](std::shared_ptr response) mutable - { + [cancel_callback, promise](std::shared_ptr response) mutable { auto cancel_response = std::static_pointer_cast(response); promise->set_value(cancel_response); if (cancel_callback) { diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 42e2844c51..dea5e508ed 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -32,15 +32,13 @@ namespace rclcpp_action { /// The possible statuses that an action goal can finish with. -enum class ResultCode : int8_t -{ +enum class ResultCode : int8_t { UNKNOWN = action_msgs::msg::GoalStatus::STATUS_UNKNOWN, SUCCEEDED = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, CANCELED = action_msgs::msg::GoalStatus::STATUS_CANCELED, ABORTED = action_msgs::msg::GoalStatus::STATUS_ABORTED }; - // Forward declarations template class Client; @@ -73,21 +71,17 @@ class ClientGoalHandle using Feedback = typename ActionT::Feedback; using Result = typename ActionT::Result; - using FeedbackCallback = - std::function::SharedPtr, - const std::shared_ptr)>; - using ResultCallback = std::function; + using FeedbackCallback = std::function::SharedPtr, const std::shared_ptr)>; + using ResultCallback = std::function; virtual ~ClientGoalHandle(); /// Get the unique ID for the goal. - const GoalUUID & - get_goal_id() const; + const GoalUUID & get_goal_id() const; /// Get the time when the goal was accepted. - rclcpp::Time - get_goal_stamp() const; + rclcpp::Time get_goal_stamp() const; /// Get a future to the goal result. /** @@ -99,53 +93,40 @@ class ClientGoalHandle * \throws exceptions::UnawareGoalHandleError If the the goal handle is unaware of the result. * \return A future to the result. */ - std::shared_future - async_result(); + std::shared_future async_result(); /// Get the goal status code. - int8_t - get_status(); + int8_t get_status(); /// Check if an action client has subscribed to feedback for the goal. - bool - is_feedback_aware(); + bool is_feedback_aware(); /// Check if an action client has requested the result for the goal. - bool - is_result_aware(); + bool is_result_aware(); private: // The templated Client creates goal handles friend Client; ClientGoalHandle( - const GoalInfo & info, - FeedbackCallback feedback_callback, - ResultCallback result_callback); + const GoalInfo & info, FeedbackCallback feedback_callback, ResultCallback result_callback); - void - set_feedback_callback(FeedbackCallback callback); + void set_feedback_callback(FeedbackCallback callback); - void - set_result_callback(ResultCallback callback); + void set_result_callback(ResultCallback callback); - void - call_feedback_callback( + void call_feedback_callback( typename ClientGoalHandle::SharedPtr shared_this, typename std::shared_ptr feedback_message); /// Returns the previous value of awareness - bool - set_result_awareness(bool awareness); + bool set_result_awareness(bool awareness); - void - set_status(int8_t status); + void set_status(int8_t status); - void - set_result(const WrappedResult & wrapped_result); + void set_result(const WrappedResult & wrapped_result); - void - invalidate(); + void invalidate(); GoalInfo info_; @@ -162,4 +143,4 @@ class ClientGoalHandle } // namespace rclcpp_action #include // NOLINT(build/include_order) -#endif // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ +#endif // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 402bd25e2e..55b18fc3cd 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -42,15 +42,13 @@ ClientGoalHandle::~ClientGoalHandle() } template -const GoalUUID & -ClientGoalHandle::get_goal_id() const +const GoalUUID & ClientGoalHandle::get_goal_id() const { return info_.goal_id.uuid; } template -rclcpp::Time -ClientGoalHandle::get_goal_stamp() const +rclcpp::Time ClientGoalHandle::get_goal_stamp() const { return info_.stamp; } @@ -67,8 +65,7 @@ ClientGoalHandle::async_result() } template -void -ClientGoalHandle::set_result(const WrappedResult & wrapped_result) +void ClientGoalHandle::set_result(const WrappedResult & wrapped_result) { std::lock_guard guard(handle_mutex_); status_ = static_cast(wrapped_result.code); @@ -79,56 +76,49 @@ ClientGoalHandle::set_result(const WrappedResult & wrapped_result) } template -void -ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) +void ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) { std::lock_guard guard(handle_mutex_); feedback_callback_ = callback; } template -void -ClientGoalHandle::set_result_callback(ResultCallback callback) +void ClientGoalHandle::set_result_callback(ResultCallback callback) { std::lock_guard guard(handle_mutex_); result_callback_ = callback; } template -int8_t -ClientGoalHandle::get_status() +int8_t ClientGoalHandle::get_status() { std::lock_guard guard(handle_mutex_); return status_; } template -void -ClientGoalHandle::set_status(int8_t status) +void ClientGoalHandle::set_status(int8_t status) { std::lock_guard guard(handle_mutex_); status_ = status; } template -bool -ClientGoalHandle::is_feedback_aware() +bool ClientGoalHandle::is_feedback_aware() { std::lock_guard guard(handle_mutex_); return feedback_callback_ != nullptr; } template -bool -ClientGoalHandle::is_result_aware() +bool ClientGoalHandle::is_result_aware() { std::lock_guard guard(handle_mutex_); return is_result_aware_; } template -bool -ClientGoalHandle::set_result_awareness(bool awareness) +bool ClientGoalHandle::set_result_awareness(bool awareness) { std::lock_guard guard(handle_mutex_); bool previous = is_result_aware_; @@ -137,8 +127,7 @@ ClientGoalHandle::set_result_awareness(bool awareness) } template -void -ClientGoalHandle::invalidate() +void ClientGoalHandle::invalidate() { std::lock_guard guard(handle_mutex_); status_ = GoalStatus::STATUS_UNKNOWN; @@ -146,8 +135,7 @@ ClientGoalHandle::invalidate() } template -void -ClientGoalHandle::call_feedback_callback( +void ClientGoalHandle::call_feedback_callback( typename ClientGoalHandle::SharedPtr shared_this, typename std::shared_ptr feedback_message) { diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 762bc5b380..b43b049d85 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -39,8 +39,7 @@ namespace rclcpp_action * If `nullptr`, then the action client is added to the default callback group. */ template -typename Client::SharedPtr -create_client( +typename Client::SharedPtr create_client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, @@ -53,37 +52,32 @@ create_client( std::weak_ptr weak_group = group; bool group_is_null = (nullptr == group.get()); - auto deleter = [weak_node, weak_group, group_is_null](Client * ptr) - { - if (nullptr == ptr) { - return; - } - auto shared_node = weak_node.lock(); - if (!shared_node) { - return; - } - // API expects a shared pointer, give it one with a deleter that does nothing. - std::shared_ptr> fake_shared_ptr(ptr, [](Client *) {}); + auto deleter = [weak_node, weak_group, group_is_null](Client * ptr) { + if (nullptr == ptr) { + return; + } + auto shared_node = weak_node.lock(); + if (!shared_node) { + return; + } + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr> fake_shared_ptr(ptr, [](Client *) {}); - if (group_is_null) { - // Was added to default group - shared_node->remove_waitable(fake_shared_ptr, nullptr); - } else { - // Was added to a specfic group - auto shared_group = weak_group.lock(); - if (shared_group) { - shared_node->remove_waitable(fake_shared_ptr, shared_group); - } + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specfic group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); } - delete ptr; - }; + } + delete ptr; + }; std::shared_ptr> action_client( - new Client( - node_base_interface, - node_graph_interface, - node_logging_interface, - name), + new Client(node_base_interface, node_graph_interface, node_logging_interface, name), deleter); node_waitables_interface->add_waitable(action_client, group); @@ -98,8 +92,7 @@ create_client( * If `nullptr`, then the action client is added to the default callback group. */ template -typename Client::SharedPtr -create_client( +typename Client::SharedPtr create_client( rclcpp::Node::SharedPtr node, const std::string & name, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 604011fc6c..6b728390be 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -54,8 +54,7 @@ namespace rclcpp_action * If `nullptr`, then the action server is added to the default callback group. */ template -typename Server::SharedPtr -create_server( +typename Server::SharedPtr create_server( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, @@ -72,32 +71,32 @@ create_server( std::weak_ptr weak_group = group; bool group_is_null = (nullptr == group.get()); - auto deleter = [weak_node, weak_group, group_is_null](Server * ptr) - { - if (nullptr == ptr) { - return; - } - auto shared_node = weak_node.lock(); - if (!shared_node) { - return; - } - // API expects a shared pointer, give it one with a deleter that does nothing. - std::shared_ptr> fake_shared_ptr(ptr, [](Server *) {}); + auto deleter = [weak_node, weak_group, group_is_null](Server * ptr) { + if (nullptr == ptr) { + return; + } + auto shared_node = weak_node.lock(); + if (!shared_node) { + return; + } + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr> fake_shared_ptr(ptr, [](Server *) {}); - if (group_is_null) { - // Was added to default group - shared_node->remove_waitable(fake_shared_ptr, nullptr); - } else { - // Was added to a specfic group - auto shared_group = weak_group.lock(); - if (shared_group) { - shared_node->remove_waitable(fake_shared_ptr, shared_group); - } + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specfic group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); } - delete ptr; - }; + } + delete ptr; + }; - std::shared_ptr> action_server(new Server( + std::shared_ptr> action_server( + new Server( node_base_interface, node_clock_interface, node_logging_interface, @@ -105,7 +104,8 @@ create_server( options, handle_goal, handle_cancel, - handle_accepted), deleter); + handle_accepted), + deleter); node_waitables_interface->add_waitable(action_server, group); return action_server; @@ -129,8 +129,7 @@ create_server( * If `nullptr`, then the action server is added to the default callback group. */ template -typename Server::SharedPtr -create_server( +typename Server::SharedPtr create_server( rclcpp::Node::SharedPtr node, const std::string & name, typename Server::GoalCallback handle_goal, diff --git a/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp_action/include/rclcpp_action/exceptions.hpp index 33d94c2804..aa1cb8a2ff 100644 --- a/rclcpp_action/include/rclcpp_action/exceptions.hpp +++ b/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -24,8 +24,7 @@ namespace exceptions class UnknownGoalHandleError : public std::invalid_argument { public: - UnknownGoalHandleError() - : std::invalid_argument("Goal handle is not known to this client.") + UnknownGoalHandleError() : std::invalid_argument("Goal handle is not known to this client.") { } }; @@ -33,8 +32,7 @@ class UnknownGoalHandleError : public std::invalid_argument class UnawareGoalHandleError : public std::runtime_error { public: - UnawareGoalHandleError() - : std::runtime_error("Goal handle is not tracking the goal result.") + UnawareGoalHandleError() : std::runtime_error("Goal handle is not tracking the goal result.") { } }; diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 7b618a0993..a7868a743f 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -17,11 +17,11 @@ #include #include -#include #include #include #include #include +#include #include #include @@ -30,9 +30,9 @@ #include #include -#include "rclcpp_action/visibility_control.hpp" #include "rclcpp_action/server_goal_handle.hpp" #include "rclcpp_action/types.hpp" +#include "rclcpp_action/visibility_control.hpp" namespace rclcpp_action { @@ -40,8 +40,7 @@ namespace rclcpp_action class ServerBaseImpl; /// A response returned by an action server callback when a goal is requested. -enum class GoalResponse : int8_t -{ +enum class GoalResponse : int8_t { /// The goal is rejected and will not be executed. REJECT = 1, /// The server accepts the goal, and is going to begin execution immediately. @@ -51,8 +50,7 @@ enum class GoalResponse : int8_t }; /// A response returned by an action server callback when a goal has been asked to be canceled. -enum class CancelResponse : int8_t -{ +enum class CancelResponse : int8_t { /// The server will not try to cancel the goal. REJECT = 1, /// The server has agreed to try to cancel the goal. @@ -79,50 +77,42 @@ class ServerBase : public rclcpp::Waitable /// Return the number of subscriptions used to implement an action server /// \internal RCLCPP_ACTION_PUBLIC - size_t - get_number_of_ready_subscriptions() override; + size_t get_number_of_ready_subscriptions() override; /// Return the number of timers used to implement an action server /// \internal RCLCPP_ACTION_PUBLIC - size_t - get_number_of_ready_timers() override; + size_t get_number_of_ready_timers() override; /// Return the number of service clients used to implement an action server /// \internal RCLCPP_ACTION_PUBLIC - size_t - get_number_of_ready_clients() override; + size_t get_number_of_ready_clients() override; /// Return the number of service servers used to implement an action server /// \internal RCLCPP_ACTION_PUBLIC - size_t - get_number_of_ready_services() override; + size_t get_number_of_ready_services() override; /// Return the number of guard conditions used to implement an action server /// \internal RCLCPP_ACTION_PUBLIC - size_t - get_number_of_ready_guard_conditions() override; + size_t get_number_of_ready_guard_conditions() override; /// Add all entities to a wait set. /// \internal RCLCPP_ACTION_PUBLIC - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; + bool add_to_wait_set(rcl_wait_set_t * wait_set) override; /// Return true if any entity belonging to the action server is ready to be executed. /// \internal RCLCPP_ACTION_PUBLIC - bool - is_ready(rcl_wait_set_t *) override; + bool is_ready(rcl_wait_set_t *) override; /// Act on entities in the wait set which are ready to be acted upon. /// \internal RCLCPP_ACTION_PUBLIC - void - execute() override; + void execute() override; // End Waitables API // ----------------- @@ -144,82 +134,65 @@ class ServerBase : public rclcpp::Waitable // The subclass should convert to the real type and call a user's callback. /// \internal RCLCPP_ACTION_PUBLIC - virtual - std::pair> - call_handle_goal_callback(GoalUUID &, std::shared_ptr request) = 0; + virtual std::pair> call_handle_goal_callback( + GoalUUID &, std::shared_ptr request) = 0; // ServerBase will determine which goal ids are being cancelled, and then call this function for // each goal id. // The subclass should look up a goal handle and call the user's callback. /// \internal RCLCPP_ACTION_PUBLIC - virtual - CancelResponse - call_handle_cancel_callback(const GoalUUID & uuid) = 0; + virtual CancelResponse call_handle_cancel_callback(const GoalUUID & uuid) = 0; /// Given a goal request message, return the UUID contained within. /// \internal RCLCPP_ACTION_PUBLIC - virtual - GoalUUID - get_goal_id_from_goal_request(void * message) = 0; + virtual GoalUUID get_goal_id_from_goal_request(void * message) = 0; /// Create an empty goal request message so it can be taken from a lower layer. /// \internal RCLCPP_ACTION_PUBLIC - virtual - std::shared_ptr - create_goal_request() = 0; + virtual std::shared_ptr create_goal_request() = 0; /// Call user callback to inform them a goal has been accepted. /// \internal RCLCPP_ACTION_PUBLIC - virtual - void - call_goal_accepted_callback( + virtual void call_goal_accepted_callback( std::shared_ptr rcl_goal_handle, - GoalUUID uuid, std::shared_ptr goal_request_message) = 0; + GoalUUID uuid, + std::shared_ptr goal_request_message) = 0; /// Given a result request message, return the UUID contained within. /// \internal RCLCPP_ACTION_PUBLIC - virtual - GoalUUID - get_goal_id_from_result_request(void * message) = 0; + virtual GoalUUID get_goal_id_from_result_request(void * message) = 0; /// Create an empty goal request message so it can be taken from a lower layer. /// \internal RCLCPP_ACTION_PUBLIC - virtual - std::shared_ptr - create_result_request() = 0; + virtual std::shared_ptr create_result_request() = 0; /// Create an empty goal result message so it can be sent as a reply in a lower layer /// \internal RCLCPP_ACTION_PUBLIC - virtual - std::shared_ptr - create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0; + virtual std::shared_ptr create_result_response( + decltype(action_msgs::msg::GoalStatus::status) status) = 0; /// \internal RCLCPP_ACTION_PUBLIC - void - publish_status(); + void publish_status(); /// \internal RCLCPP_ACTION_PUBLIC - void - notify_goal_terminal_state(); + void notify_goal_terminal_state(); /// \internal RCLCPP_ACTION_PUBLIC - void - publish_result(const GoalUUID & uuid, std::shared_ptr result_msg); + void publish_result(const GoalUUID & uuid, std::shared_ptr result_msg); /// \internal RCLCPP_ACTION_PUBLIC - void - publish_feedback(std::shared_ptr feedback_msg); + void publish_feedback(std::shared_ptr feedback_msg); // End API for communication between ServerBase and Server<> // --------------------------------------------------------- @@ -228,26 +201,22 @@ class ServerBase : public rclcpp::Waitable /// Handle a request to add a new goal to the server /// \internal RCLCPP_ACTION_PUBLIC - void - execute_goal_request_received(); + void execute_goal_request_received(); /// Handle a request to cancel goals on the server /// \internal RCLCPP_ACTION_PUBLIC - void - execute_cancel_request_received(); + void execute_cancel_request_received(); /// Handle a request to get the result of an action /// \internal RCLCPP_ACTION_PUBLIC - void - execute_result_request_received(); + void execute_result_request_received(); /// Handle a timeout indicating a completed goal should be forgotten by the server /// \internal RCLCPP_ACTION_PUBLIC - void - execute_check_expired_goals(); + void execute_check_expired_goals(); /// Private implementation /// \internal @@ -271,12 +240,12 @@ class Server : public ServerBase, public std::enable_shared_from_this)>; + using GoalCallback = + std::function)>; /// Signature of a callback that accepts or rejects requests to cancel a goal. using CancelCallback = std::function>)>; /// Signature of a callback that is used to notify when the goal has been accepted. - using AcceptedCallback = std::function>)>; + using AcceptedCallback = std::function>)>; /// Construct an action server. /** @@ -312,8 +281,7 @@ class Server : public ServerBase, public std::enable_shared_from_this /// \internal - std::pair> - call_handle_goal_callback(GoalUUID & uuid, std::shared_ptr message) override + std::pair> call_handle_goal_callback( + GoalUUID & uuid, std::shared_ptr message) override { - auto request = std::static_pointer_cast< - typename ActionT::Impl::SendGoalService::Request>(message); + auto request = + std::static_pointer_cast(message); auto goal = std::shared_ptr(request, &request->goal); GoalResponse user_response = handle_goal_(uuid, goal); auto ros_response = std::make_shared(); ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response || - GoalResponse::ACCEPT_AND_DEFER == user_response; + GoalResponse::ACCEPT_AND_DEFER == user_response; return std::make_pair(user_response, ros_response); } /// \internal - CancelResponse - call_handle_cancel_callback(const GoalUUID & uuid) override + CancelResponse call_handle_cancel_callback(const GoalUUID & uuid) override { std::lock_guard lock(goal_handles_mutex_); CancelResponse resp = CancelResponse::REJECT; @@ -368,17 +335,16 @@ class Server : public ServerBase, public std::enable_shared_from_this rcl_goal_handle, - GoalUUID uuid, std::shared_ptr goal_request_message) override + GoalUUID uuid, + std::shared_ptr goal_request_message) override { std::shared_ptr> goal_handle; std::weak_ptr> weak_this = this->shared_from_this(); std::function)> on_terminal_state = - [weak_this](const GoalUUID & uuid, std::shared_ptr result_message) - { + [weak_this](const GoalUUID & uuid, std::shared_ptr result_message) { std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { return; @@ -394,21 +360,18 @@ class Server : public ServerBase, public std::enable_shared_from_thisgoal_handles_.erase(uuid); }; - std::function on_executing = - [weak_this](const GoalUUID & uuid) - { - std::shared_ptr> shared_this = weak_this.lock(); - if (!shared_this) { - return; - } - (void)uuid; - // Publish a status message any time a goal handle changes state - shared_this->publish_status(); - }; + std::function on_executing = [weak_this](const GoalUUID & uuid) { + std::shared_ptr> shared_this = weak_this.lock(); + if (!shared_this) { + return; + } + (void)uuid; + // Publish a status message any time a goal handle changes state + shared_this->publish_status(); + }; std::function)> publish_feedback = - [weak_this](std::shared_ptr feedback_msg) - { + [weak_this](std::shared_ptr feedback_msg) { std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { return; @@ -416,12 +379,11 @@ class Server : public ServerBase, public std::enable_shared_from_thispublish_feedback(std::static_pointer_cast(feedback_msg)); }; - auto request = std::static_pointer_cast< - const typename ActionT::Impl::SendGoalService::Request>(goal_request_message); + auto request = std::static_pointer_cast( + goal_request_message); auto goal = std::shared_ptr(request, &request->goal); - goal_handle.reset( - new ServerGoalHandle( - rcl_goal_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback)); + goal_handle.reset(new ServerGoalHandle( + rcl_goal_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback)); { std::lock_guard lock(goal_handles_mutex_); goal_handles_[uuid] = goal_handle; @@ -430,38 +392,32 @@ class Server : public ServerBase, public std::enable_shared_from_this(message)->goal_id.uuid; + return static_cast(message)->goal_id.uuid; } /// \internal - std::shared_ptr - create_goal_request() override + std::shared_ptr create_goal_request() override { return std::shared_ptr(new typename ActionT::Impl::SendGoalService::Request()); } /// \internal - GoalUUID - get_goal_id_from_result_request(void * message) override + GoalUUID get_goal_id_from_result_request(void * message) override { - return - static_cast(message)->goal_id.uuid; + return static_cast(message)->goal_id.uuid; } /// \internal - std::shared_ptr - create_result_request() override + std::shared_ptr create_result_request() override { return std::shared_ptr(new typename ActionT::Impl::GetResultService::Request()); } /// \internal - std::shared_ptr - create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override + std::shared_ptr create_result_response( + decltype(action_msgs::msg::GoalStatus::status) status) override { auto result = std::make_shared(); result->status = status; diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index d41b32eb4f..ed6572b05a 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -15,8 +15,8 @@ #ifndef RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ #define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ -#include #include +#include #include @@ -24,8 +24,8 @@ #include #include -#include "rclcpp_action/visibility_control.hpp" #include "rclcpp_action/types.hpp" +#include "rclcpp_action/visibility_control.hpp" namespace rclcpp_action { @@ -45,24 +45,20 @@ class ServerGoalHandleBase /// Indicate if client has requested this goal be cancelled. /// \return true if a cancelation request has been accepted for this goal. RCLCPP_ACTION_PUBLIC - bool - is_canceling() const; + bool is_canceling() const; /// Indicate if goal is pending or executing. /// \return false if goal has reached a terminal state. RCLCPP_ACTION_PUBLIC - bool - is_active() const; + bool is_active() const; /// Indicate if goal is executing. /// \return true only if the goal is in an executing state. RCLCPP_ACTION_PUBLIC - bool - is_executing() const; + bool is_executing() const; RCLCPP_ACTION_PUBLIC - virtual - ~ServerGoalHandleBase(); + virtual ~ServerGoalHandleBase(); protected: // ------------------------------------------------------------------------- @@ -70,43 +66,35 @@ class ServerGoalHandleBase /// \internal RCLCPP_ACTION_PUBLIC - ServerGoalHandleBase( - std::shared_ptr rcl_handle - ) + ServerGoalHandleBase(std::shared_ptr rcl_handle) : rcl_handle_(rcl_handle) { } /// \internal RCLCPP_ACTION_PUBLIC - void - _abort(); + void _abort(); /// \internal RCLCPP_ACTION_PUBLIC - void - _succeed(); + void _succeed(); /// \internal RCLCPP_ACTION_PUBLIC - void - _cancel_goal(); + void _cancel_goal(); /// \internal RCLCPP_ACTION_PUBLIC - void - _canceled(); + void _canceled(); /// \internal RCLCPP_ACTION_PUBLIC - void - _execute(); + void _execute(); /// Transition the goal to canceled state if it never reached a terminal state. /// \internal RCLCPP_ACTION_PUBLIC - bool - try_canceling() noexcept; + bool try_canceling() noexcept; // End API for communication between ServerGoalHandleBase and ServerGoalHandle<> // ----------------------------------------------------------------------------- @@ -145,8 +133,7 @@ class ServerGoalHandle : public ServerGoalHandleBase * * \param[in] feedback_msg the message to publish to clients. */ - void - publish_feedback(std::shared_ptr feedback_msg) + void publish_feedback(std::shared_ptr feedback_msg) { auto feedback_message = std::make_shared(); feedback_message->goal_id.uuid = uuid_; @@ -164,8 +151,7 @@ class ServerGoalHandle : public ServerGoalHandleBase * * \param[in] result_msg the final result to send to clients. */ - void - abort(typename ActionT::Result::SharedPtr result_msg) + void abort(typename ActionT::Result::SharedPtr result_msg) { _abort(); auto response = std::make_shared(); @@ -184,8 +170,7 @@ class ServerGoalHandle : public ServerGoalHandleBase * * \param[in] result_msg the final result to send to clients. */ - void - succeed(typename ActionT::Result::SharedPtr result_msg) + void succeed(typename ActionT::Result::SharedPtr result_msg) { _succeed(); auto response = std::make_shared(); @@ -204,8 +189,7 @@ class ServerGoalHandle : public ServerGoalHandleBase * * \param[in] result_msg the final result to send to clients. */ - void - canceled(typename ActionT::Result::SharedPtr result_msg) + void canceled(typename ActionT::Result::SharedPtr result_msg) { _canceled(); auto response = std::make_shared(); @@ -220,23 +204,20 @@ class ServerGoalHandle : public ServerGoalHandleBase * * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. */ - void - execute() + void execute() { _execute(); on_executing_(uuid_); } /// Get the user provided message describing the goal. - const std::shared_ptr - get_goal() const + const std::shared_ptr get_goal() const { return goal_; } /// Get the unique identifier of the goal - const GoalUUID & - get_goal_id() const + const GoalUUID & get_goal_id() const { return uuid_; } @@ -259,10 +240,12 @@ class ServerGoalHandle : public ServerGoalHandleBase std::shared_ptr goal, std::function)> on_terminal_state, std::function on_executing, - std::function)> publish_feedback - ) - : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid), - on_terminal_state_(on_terminal_state), on_executing_(on_executing), + std::function)> publish_feedback) + : ServerGoalHandleBase(rcl_handle), + goal_(goal), + uuid_(uuid), + on_terminal_state_(on_terminal_state), + on_executing_(on_executing), publish_feedback_(publish_feedback) { } diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index addf4a8307..1d24a3f522 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -17,8 +17,8 @@ #include -#include #include +#include #include #include @@ -35,18 +35,15 @@ using GoalInfo = action_msgs::msg::GoalInfo; /// Convert a goal id to a human readable string. RCLCPP_ACTION_PUBLIC -std::string -to_string(const GoalUUID & goal_id); +std::string to_string(const GoalUUID & goal_id); // Convert C++ GoalID to rcl_action_goal_info_t RCLCPP_ACTION_PUBLIC -void -convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info); +void convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info); // Convert rcl_action_goal_info_t to C++ GoalID RCLCPP_ACTION_PUBLIC -void -convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id); +void convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id); } // namespace rclcpp_action namespace std @@ -54,9 +51,7 @@ namespace std template<> struct less { - bool operator()( - const rclcpp_action::GoalUUID & lhs, - const rclcpp_action::GoalUUID & rhs) const + bool operator()(const rclcpp_action::GoalUUID & lhs, const rclcpp_action::GoalUUID & rhs) const { return lhs < rhs; } diff --git a/rclcpp_action/include/rclcpp_action/visibility_control.hpp b/rclcpp_action/include/rclcpp_action/visibility_control.hpp index 32b03dd8f5..143f70b472 100644 --- a/rclcpp_action/include/rclcpp_action/visibility_control.hpp +++ b/rclcpp_action/include/rclcpp_action/visibility_control.hpp @@ -26,31 +26,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define RCLCPP_ACTION_EXPORT __attribute__ ((dllexport)) - #define RCLCPP_ACTION_IMPORT __attribute__ ((dllimport)) - #else - #define RCLCPP_ACTION_EXPORT __declspec(dllexport) - #define RCLCPP_ACTION_IMPORT __declspec(dllimport) - #endif - #ifdef RCLCPP_ACTION_BUILDING_LIBRARY - #define RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_EXPORT - #else - #define RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_IMPORT - #endif - #define RCLCPP_ACTION_PUBLIC_TYPE RCLCPP_ACTION_PUBLIC - #define RCLCPP_ACTION_LOCAL +#ifdef __GNUC__ +#define RCLCPP_ACTION_EXPORT __attribute__((dllexport)) +#define RCLCPP_ACTION_IMPORT __attribute__((dllimport)) #else - #define RCLCPP_ACTION_EXPORT __attribute__ ((visibility("default"))) - #define RCLCPP_ACTION_IMPORT - #if __GNUC__ >= 4 - #define RCLCPP_ACTION_PUBLIC __attribute__ ((visibility("default"))) - #define RCLCPP_ACTION_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define RCLCPP_ACTION_PUBLIC - #define RCLCPP_ACTION_LOCAL - #endif - #define RCLCPP_ACTION_PUBLIC_TYPE +#define RCLCPP_ACTION_EXPORT __declspec(dllexport) +#define RCLCPP_ACTION_IMPORT __declspec(dllimport) +#endif +#ifdef RCLCPP_ACTION_BUILDING_LIBRARY +#define RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_EXPORT +#else +#define RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_IMPORT +#endif +#define RCLCPP_ACTION_PUBLIC_TYPE RCLCPP_ACTION_PUBLIC +#define RCLCPP_ACTION_LOCAL +#else +#define RCLCPP_ACTION_EXPORT __attribute__((visibility("default"))) +#define RCLCPP_ACTION_IMPORT +#if __GNUC__ >= 4 +#define RCLCPP_ACTION_PUBLIC __attribute__((visibility("default"))) +#define RCLCPP_ACTION_LOCAL __attribute__((visibility("hidden"))) +#else +#define RCLCPP_ACTION_PUBLIC +#define RCLCPP_ACTION_LOCAL +#endif +#define RCLCPP_ACTION_PUBLIC_TYPE #endif #endif // RCLCPP_ACTION__VISIBILITY_CONTROL_HPP_ diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 3a34f35195..5f159c3911 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -42,18 +42,18 @@ class ClientBaseImpl : node_graph_(node_graph), node_handle(node_base->get_shared_rcl_node_handle()), logger(node_logging->get_logger().get_child("rclcpp_acton")), - random_bytes_generator(std::random_device{} ()) + random_bytes_generator(std::random_device{}()) { std::weak_ptr weak_node_handle(node_handle); client_handle = std::shared_ptr( - new rcl_action_client_t, [weak_node_handle](rcl_action_client_t * client) - { + new rcl_action_client_t, [weak_node_handle](rcl_action_client_t * client) { auto handle = weak_node_handle.lock(); if (handle) { if (RCL_RET_OK != rcl_action_client_fini(client, handle.get())) { RCLCPP_ERROR( rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp_action"), - "Error in destruction of rcl action client handle: %s", rcl_get_error_string().str); + "Error in destruction of rcl action client handle: %s", + rcl_get_error_string().str); rcl_reset_error(); } } else { @@ -66,11 +66,9 @@ class ClientBaseImpl }); *client_handle = rcl_action_get_zero_initialized_client(); rcl_ret_t ret = rcl_action_client_init( - client_handle.get(), node_handle.get(), type_support, - action_name.c_str(), &client_options); + client_handle.get(), node_handle.get(), type_support, action_name.c_str(), &client_options); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "could not initialize rcl action client"); + rclcpp::exceptions::throw_from_rcl_error(ret, "could not initialize rcl action client"); } ret = rcl_action_client_wait_set_get_num_entities( @@ -81,8 +79,7 @@ class ClientBaseImpl &num_clients, &num_services); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "could not retrieve rcl action client details"); + rclcpp::exceptions::throw_from_rcl_error(ret, "could not retrieve rcl action client details"); } } @@ -104,7 +101,7 @@ class ClientBaseImpl std::shared_ptr node_handle{nullptr}; rclcpp::Logger logger; - using ResponseCallback = std::function response)>; + using ResponseCallback = std::function response)>; std::map pending_goal_responses; std::mutex goal_requests_mutex; @@ -115,8 +112,7 @@ class ClientBaseImpl std::map pending_cancel_responses; std::mutex cancel_requests_mutex; - std::independent_bits_engine< - std::default_random_engine, 8, unsigned int> random_bytes_generator; + std::independent_bits_engine random_bytes_generator; }; ClientBase::ClientBase( @@ -127,7 +123,7 @@ ClientBase::ClientBase( const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) : pimpl_(new ClientBaseImpl( - node_base, node_graph, node_logging, action_name, type_support, client_options)) + node_base, node_graph, node_logging, action_name, type_support, client_options)) { } @@ -135,14 +131,11 @@ ClientBase::~ClientBase() { } -bool -ClientBase::action_server_is_ready() const +bool ClientBase::action_server_is_ready() const { bool is_ready; rcl_ret_t ret = rcl_action_server_is_available( - this->pimpl_->node_handle.get(), - this->pimpl_->client_handle.get(), - &is_ready); + this->pimpl_->node_handle.get(), this->pimpl_->client_handle.get(), &is_ready); if (RCL_RET_NODE_INVALID == ret) { const rcl_node_t * node_handle = this->pimpl_->node_handle.get(); if (node_handle && !rcl_context_is_valid(node_handle->context)) { @@ -156,8 +149,7 @@ ClientBase::action_server_is_ready() const return is_ready; } -bool -ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) +bool ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) { auto start = std::chrono::steady_clock::now(); // make an event to reuse, rather than create a new one each time @@ -176,10 +168,9 @@ ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) } // update the time even on the first loop to account for time spent in the first call // to this->server_is_ready() - std::chrono::nanoseconds time_to_wait = - timeout > std::chrono::nanoseconds(0) ? - timeout - (std::chrono::steady_clock::now() - start) : - std::chrono::nanoseconds::max(); + std::chrono::nanoseconds time_to_wait = timeout > std::chrono::nanoseconds(0) ? + timeout - (std::chrono::steady_clock::now() - start) : + std::chrono::nanoseconds::max(); if (time_to_wait < std::chrono::nanoseconds(0)) { // Do not allow the time_to_wait to become negative when timeout was originally positive. // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while. @@ -213,76 +204,62 @@ ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) return false; // timeout exceeded while waiting for the server to be ready } -rclcpp::Logger -ClientBase::get_logger() +rclcpp::Logger ClientBase::get_logger() { return pimpl_->logger; } -size_t -ClientBase::get_number_of_ready_subscriptions() +size_t ClientBase::get_number_of_ready_subscriptions() { return pimpl_->num_subscriptions; } -size_t -ClientBase::get_number_of_ready_guard_conditions() +size_t ClientBase::get_number_of_ready_guard_conditions() { return pimpl_->num_guard_conditions; } -size_t -ClientBase::get_number_of_ready_timers() +size_t ClientBase::get_number_of_ready_timers() { return pimpl_->num_timers; } -size_t -ClientBase::get_number_of_ready_clients() +size_t ClientBase::get_number_of_ready_clients() { return pimpl_->num_clients; } -size_t -ClientBase::get_number_of_ready_services() +size_t ClientBase::get_number_of_ready_services() { return pimpl_->num_services; } -bool -ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) +bool ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) { - rcl_ret_t ret = rcl_action_wait_set_add_action_client( - wait_set, pimpl_->client_handle.get(), nullptr, nullptr); + rcl_ret_t ret = + rcl_action_wait_set_add_action_client(wait_set, pimpl_->client_handle.get(), nullptr, nullptr); return RCL_RET_OK == ret; } -bool -ClientBase::is_ready(rcl_wait_set_t * wait_set) +bool ClientBase::is_ready(rcl_wait_set_t * wait_set) { rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( - wait_set, pimpl_->client_handle.get(), + wait_set, + pimpl_->client_handle.get(), &pimpl_->is_feedback_ready, &pimpl_->is_status_ready, &pimpl_->is_goal_response_ready, &pimpl_->is_cancel_response_ready, &pimpl_->is_result_response_ready); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "failed to check for any ready entities"); + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to check for any ready entities"); } - return - pimpl_->is_feedback_ready || - pimpl_->is_status_ready || - pimpl_->is_goal_response_ready || - pimpl_->is_cancel_response_ready || - pimpl_->is_result_response_ready; + return pimpl_->is_feedback_ready || pimpl_->is_status_ready || pimpl_->is_goal_response_ready || + pimpl_->is_cancel_response_ready || pimpl_->is_result_response_ready; } -void -ClientBase::handle_goal_response( - const rmw_request_id_t & response_header, - std::shared_ptr response) +void ClientBase::handle_goal_response( + const rmw_request_id_t & response_header, std::shared_ptr response) { std::lock_guard guard(pimpl_->goal_requests_mutex); const int64_t & sequence_number = response_header.sequence_number; @@ -294,13 +271,12 @@ ClientBase::handle_goal_response( pimpl_->pending_goal_responses.erase(sequence_number); } -void -ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback callback) +void ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback callback) { std::unique_lock guard(pimpl_->goal_requests_mutex); int64_t sequence_number; - rcl_ret_t ret = rcl_action_send_goal_request( - pimpl_->client_handle.get(), request.get(), &sequence_number); + rcl_ret_t ret = + rcl_action_send_goal_request(pimpl_->client_handle.get(), request.get(), &sequence_number); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); } @@ -308,10 +284,8 @@ ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback ca pimpl_->pending_goal_responses[sequence_number] = callback; } -void -ClientBase::handle_result_response( - const rmw_request_id_t & response_header, - std::shared_ptr response) +void ClientBase::handle_result_response( + const rmw_request_id_t & response_header, std::shared_ptr response) { std::lock_guard guard(pimpl_->result_requests_mutex); const int64_t & sequence_number = response_header.sequence_number; @@ -323,13 +297,12 @@ ClientBase::handle_result_response( pimpl_->pending_result_responses.erase(sequence_number); } -void -ClientBase::send_result_request(std::shared_ptr request, ResponseCallback callback) +void ClientBase::send_result_request(std::shared_ptr request, ResponseCallback callback) { std::lock_guard guard(pimpl_->result_requests_mutex); int64_t sequence_number; - rcl_ret_t ret = rcl_action_send_result_request( - pimpl_->client_handle.get(), request.get(), &sequence_number); + rcl_ret_t ret = + rcl_action_send_result_request(pimpl_->client_handle.get(), request.get(), &sequence_number); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send result request"); } @@ -337,10 +310,8 @@ ClientBase::send_result_request(std::shared_ptr request, ResponseCallback pimpl_->pending_result_responses[sequence_number] = callback; } -void -ClientBase::handle_cancel_response( - const rmw_request_id_t & response_header, - std::shared_ptr response) +void ClientBase::handle_cancel_response( + const rmw_request_id_t & response_header, std::shared_ptr response) { std::lock_guard guard(pimpl_->cancel_requests_mutex); const int64_t & sequence_number = response_header.sequence_number; @@ -352,13 +323,12 @@ ClientBase::handle_cancel_response( pimpl_->pending_cancel_responses.erase(sequence_number); } -void -ClientBase::send_cancel_request(std::shared_ptr request, ResponseCallback callback) +void ClientBase::send_cancel_request(std::shared_ptr request, ResponseCallback callback) { std::lock_guard guard(pimpl_->cancel_requests_mutex); int64_t sequence_number; - rcl_ret_t ret = rcl_action_send_cancel_request( - pimpl_->client_handle.get(), request.get(), &sequence_number); + rcl_ret_t ret = + rcl_action_send_cancel_request(pimpl_->client_handle.get(), request.get(), &sequence_number); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send cancel request"); } @@ -366,27 +336,22 @@ ClientBase::send_cancel_request(std::shared_ptr request, ResponseCallback pimpl_->pending_cancel_responses[sequence_number] = callback; } -GoalUUID -ClientBase::generate_goal_id() +GoalUUID ClientBase::generate_goal_id() { GoalUUID goal_id; // TODO(hidmic): Do something better than this for UUID generation. // std::generate( // goal_id.uuid.begin(), goal_id.uuid.end(), // std::ref(pimpl_->random_bytes_generator)); - std::generate( - goal_id.begin(), goal_id.end(), - std::ref(pimpl_->random_bytes_generator)); + std::generate(goal_id.begin(), goal_id.end(), std::ref(pimpl_->random_bytes_generator)); return goal_id; } -void -ClientBase::execute() +void ClientBase::execute() { if (pimpl_->is_feedback_ready) { std::shared_ptr feedback_message = this->create_feedback_message(); - rcl_ret_t ret = rcl_action_take_feedback( - pimpl_->client_handle.get(), feedback_message.get()); + rcl_ret_t ret = rcl_action_take_feedback(pimpl_->client_handle.get(), feedback_message.get()); pimpl_->is_feedback_ready = false; if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback"); @@ -395,8 +360,7 @@ ClientBase::execute() } } else if (pimpl_->is_status_ready) { std::shared_ptr status_message = this->create_status_message(); - rcl_ret_t ret = rcl_action_take_status( - pimpl_->client_handle.get(), status_message.get()); + rcl_ret_t ret = rcl_action_take_status(pimpl_->client_handle.get(), status_message.get()); pimpl_->is_status_ready = false; if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status"); diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index df21d3469a..fc6425382c 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -27,18 +27,15 @@ #include #include -using rclcpp_action::ServerBase; using rclcpp_action::GoalUUID; +using rclcpp_action::ServerBase; namespace rclcpp_action { class ServerBaseImpl { public: - ServerBaseImpl( - rclcpp::Clock::SharedPtr clock, - rclcpp::Logger logger - ) + ServerBaseImpl(rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger) : clock_(clock), logger_(logger) { } @@ -78,23 +75,20 @@ ServerBase::ServerBase( rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & name, const rosidl_action_type_support_t * type_support, - const rcl_action_server_options_t & options -) + const rcl_action_server_options_t & options) : pimpl_(new ServerBaseImpl( - node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action"))) + node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action"))) { - auto deleter = [node_base](rcl_action_server_t * ptr) - { - if (nullptr != ptr) { - rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); - rcl_ret_t ret = rcl_action_server_fini(ptr, rcl_node); - (void)ret; - RCLCPP_DEBUG( - rclcpp::get_logger("rclcpp_action"), - "failed to fini rcl_action_server_t in deleter"); - } - delete ptr; - }; + auto deleter = [node_base](rcl_action_server_t * ptr) { + if (nullptr != ptr) { + rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); + rcl_ret_t ret = rcl_action_server_fini(ptr, rcl_node); + (void)ret; + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), "failed to fini rcl_action_server_t in deleter"); + } + delete ptr; + }; pimpl_->action_server_.reset(new rcl_action_server_t, deleter); *(pimpl_->action_server_) = rcl_action_get_zero_initialized_server(); @@ -126,47 +120,40 @@ ServerBase::~ServerBase() { } -size_t -ServerBase::get_number_of_ready_subscriptions() +size_t ServerBase::get_number_of_ready_subscriptions() { return pimpl_->num_subscriptions_; } -size_t -ServerBase::get_number_of_ready_timers() +size_t ServerBase::get_number_of_ready_timers() { return pimpl_->num_timers_; } -size_t -ServerBase::get_number_of_ready_clients() +size_t ServerBase::get_number_of_ready_clients() { return pimpl_->num_clients_; } -size_t -ServerBase::get_number_of_ready_services() +size_t ServerBase::get_number_of_ready_services() { return pimpl_->num_services_; } -size_t -ServerBase::get_number_of_ready_guard_conditions() +size_t ServerBase::get_number_of_ready_guard_conditions() { return pimpl_->num_guard_conditions_; } -bool -ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +bool ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { std::lock_guard lock(pimpl_->reentrant_mutex_); - rcl_ret_t ret = rcl_action_wait_set_add_action_server( - wait_set, pimpl_->action_server_.get(), NULL); + rcl_ret_t ret = + rcl_action_wait_set_add_action_server(wait_set, pimpl_->action_server_.get(), NULL); return RCL_RET_OK == ret; } -bool -ServerBase::is_ready(rcl_wait_set_t * wait_set) +bool ServerBase::is_ready(rcl_wait_set_t * wait_set) { std::lock_guard lock(pimpl_->reentrant_mutex_); rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( @@ -181,14 +168,11 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) rclcpp::exceptions::throw_from_rcl_error(ret); } - return pimpl_->goal_request_ready_ || - pimpl_->cancel_request_ready_ || - pimpl_->result_request_ready_ || - pimpl_->goal_expired_; + return pimpl_->goal_request_ready_ || pimpl_->cancel_request_ready_ || + pimpl_->result_request_ready_ || pimpl_->goal_expired_; } -void -ServerBase::execute() +void ServerBase::execute() { if (pimpl_->goal_request_ready_) { execute_goal_request_received(); @@ -203,8 +187,7 @@ ServerBase::execute() } } -void -ServerBase::execute_goal_request_received() +void ServerBase::execute_goal_request_received() { rcl_ret_t ret; rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); @@ -213,10 +196,7 @@ ServerBase::execute_goal_request_received() std::lock_guard lock(pimpl_->reentrant_mutex_); std::shared_ptr message = create_goal_request(); - ret = rcl_action_take_goal_request( - pimpl_->action_server_.get(), - &request_header, - message.get()); + ret = rcl_action_take_goal_request(pimpl_->action_server_.get(), &request_header, message.get()); pimpl_->goal_request_ready_ = false; @@ -236,9 +216,7 @@ ServerBase::execute_goal_request_received() auto response_pair = call_handle_goal_callback(uuid, message); ret = rcl_action_send_goal_response( - pimpl_->action_server_.get(), - &request_header, - response_pair.second.get()); + pimpl_->action_server_.get(), &request_header, response_pair.second.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -250,17 +228,16 @@ ServerBase::execute_goal_request_received() if (GoalResponse::ACCEPT_AND_EXECUTE == status || GoalResponse::ACCEPT_AND_DEFER == status) { RCLCPP_DEBUG(pimpl_->logger_, "Accepted goal %s", to_string(uuid).c_str()); // rcl_action will set time stamp - auto deleter = [](rcl_action_goal_handle_t * ptr) - { - if (nullptr != ptr) { - rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); - (void)fail_ret; - RCLCPP_DEBUG( - rclcpp::get_logger("rclcpp_action"), - "failed to fini rcl_action_goal_handle_t in deleter"); - delete ptr; - } - }; + auto deleter = [](rcl_action_goal_handle_t * ptr) { + if (nullptr != ptr) { + rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); + (void)fail_ret; + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), + "failed to fini rcl_action_goal_handle_t in deleter"); + delete ptr; + } + }; rcl_action_goal_handle_t * rcl_handle; rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info); if (!rcl_handle) { @@ -288,8 +265,7 @@ ServerBase::execute_goal_request_received() } } -void -ServerBase::execute_cancel_request_received() +void ServerBase::execute_cancel_request_received() { rcl_ret_t ret; rmw_request_id_t request_header; @@ -298,10 +274,8 @@ ServerBase::execute_cancel_request_received() auto request = std::make_shared(); std::lock_guard lock(pimpl_->reentrant_mutex_); - ret = rcl_action_take_cancel_request( - pimpl_->action_server_.get(), - &request_header, - request.get()); + ret = + rcl_action_take_cancel_request(pimpl_->action_server_.get(), &request_header, request.get()); pimpl_->cancel_request_ready_ = false; @@ -324,15 +298,12 @@ ServerBase::execute_cancel_request_received() rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); ret = rcl_action_process_cancel_request( - pimpl_->action_server_.get(), - &cancel_request, - &cancel_response); + pimpl_->action_server_.get(), &cancel_request, &cancel_response); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - RCLCPP_SCOPE_EXIT( - { + RCLCPP_SCOPE_EXIT({ ret = rcl_action_cancel_response_fini(&cancel_response); if (RCL_RET_OK != ret) { RCLCPP_ERROR(pimpl_->logger_, "Failed to fini cancel response"); @@ -369,15 +340,14 @@ ServerBase::execute_cancel_request_received() publish_status(); } - ret = rcl_action_send_cancel_response( - pimpl_->action_server_.get(), &request_header, response.get()); + ret = + rcl_action_send_cancel_response(pimpl_->action_server_.get(), &request_header, response.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } } -void -ServerBase::execute_result_request_received() +void ServerBase::execute_result_request_received() { rcl_ret_t ret; // Get the result request message @@ -430,8 +400,7 @@ ServerBase::execute_result_request_received() } } -void -ServerBase::execute_check_expired_goals() +void ServerBase::execute_check_expired_goals() { // Allocate expecting only one goal to expire at a time rcl_action_goal_info_t expired_goals[1]; @@ -456,16 +425,14 @@ ServerBase::execute_check_expired_goals() } } -void -ServerBase::publish_status() +void ServerBase::publish_status() { rcl_ret_t ret; // Get all goal handles known to C action server rcl_action_goal_handle_t ** goal_handles = NULL; size_t num_goals = 0; - ret = rcl_action_server_get_goal_handles( - pimpl_->action_server_.get(), &goal_handles, &num_goals); + ret = rcl_action_server_get_goal_handles(pimpl_->action_server_.get(), &goal_handles, &num_goals); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -481,8 +448,7 @@ ServerBase::publish_status() rclcpp::exceptions::throw_from_rcl_error(ret); } - RCLCPP_SCOPE_EXIT( - { + RCLCPP_SCOPE_EXIT({ ret = rcl_action_goal_status_array_fini(&c_status_array); if (RCL_RET_OK != ret) { RCLCPP_ERROR(pimpl_->logger_, "Failed to fini status array message"); @@ -510,8 +476,7 @@ ServerBase::publish_status() } } -void -ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_msg) +void ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_msg) { // Check that the goal exists rcl_action_goal_info_t goal_info; @@ -539,8 +504,7 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_m } } -void -ServerBase::notify_goal_terminal_state() +void ServerBase::notify_goal_terminal_state() { std::lock_guard lock(pimpl_->reentrant_mutex_); rcl_ret_t ret = rcl_action_notify_goal_done(pimpl_->action_server_.get()); @@ -549,8 +513,7 @@ ServerBase::notify_goal_terminal_state() } } -void -ServerBase::publish_feedback(std::shared_ptr feedback_msg) +void ServerBase::publish_feedback(std::shared_ptr feedback_msg) { std::lock_guard lock(pimpl_->reentrant_mutex_); rcl_ret_t ret = rcl_action_publish_feedback(pimpl_->action_server_.get(), feedback_msg.get()); diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index f0709e19cb..d15f148d75 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -15,8 +15,8 @@ #include #include -#include #include +#include #include @@ -26,8 +26,7 @@ ServerGoalHandleBase::~ServerGoalHandleBase() { } -bool -ServerGoalHandleBase::is_canceling() const +bool ServerGoalHandleBase::is_canceling() const { std::lock_guard lock(rcl_handle_mutex_); rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; @@ -38,15 +37,13 @@ ServerGoalHandleBase::is_canceling() const return GOAL_STATE_CANCELING == state; } -bool -ServerGoalHandleBase::is_active() const +bool ServerGoalHandleBase::is_active() const { std::lock_guard lock(rcl_handle_mutex_); return rcl_action_goal_handle_is_active(rcl_handle_.get()); } -bool -ServerGoalHandleBase::is_executing() const +bool ServerGoalHandleBase::is_executing() const { std::lock_guard lock(rcl_handle_mutex_); rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; @@ -57,8 +54,7 @@ ServerGoalHandleBase::is_executing() const return GOAL_STATE_EXECUTING == state; } -void -ServerGoalHandleBase::_abort() +void ServerGoalHandleBase::_abort() { std::lock_guard lock(rcl_handle_mutex_); rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_ABORT); @@ -67,8 +63,7 @@ ServerGoalHandleBase::_abort() } } -void -ServerGoalHandleBase::_succeed() +void ServerGoalHandleBase::_succeed() { std::lock_guard lock(rcl_handle_mutex_); rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SUCCEED); @@ -77,8 +72,7 @@ ServerGoalHandleBase::_succeed() } } -void -ServerGoalHandleBase::_cancel_goal() +void ServerGoalHandleBase::_cancel_goal() { std::lock_guard lock(rcl_handle_mutex_); rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL); @@ -87,8 +81,7 @@ ServerGoalHandleBase::_cancel_goal() } } -void -ServerGoalHandleBase::_canceled() +void ServerGoalHandleBase::_canceled() { std::lock_guard lock(rcl_handle_mutex_); rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED); @@ -97,8 +90,7 @@ ServerGoalHandleBase::_canceled() } } -void -ServerGoalHandleBase::_execute() +void ServerGoalHandleBase::_execute() { std::lock_guard lock(rcl_handle_mutex_); rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_EXECUTE); @@ -107,8 +99,7 @@ ServerGoalHandleBase::_execute() } } -bool -ServerGoalHandleBase::try_canceling() noexcept +bool ServerGoalHandleBase::try_canceling() noexcept { std::lock_guard lock(rcl_handle_mutex_); rcl_ret_t ret; diff --git a/rclcpp_action/src/types.cpp b/rclcpp_action/src/types.cpp index 773702789e..61d642d794 100644 --- a/rclcpp_action/src/types.cpp +++ b/rclcpp_action/src/types.cpp @@ -14,13 +14,12 @@ #include "rclcpp_action/types.hpp" -#include #include +#include namespace rclcpp_action { -std::string -to_string(const GoalUUID & goal_id) +std::string to_string(const GoalUUID & goal_id) { std::stringstream stream; stream << std::hex; @@ -30,16 +29,14 @@ to_string(const GoalUUID & goal_id) return stream.str(); } -void -convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info) +void convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info) { for (size_t i = 0; i < 16; ++i) { info->goal_id.uuid[i] = goal_id[i]; } } -void -convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id) +void convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id) { for (size_t i = 0; i < 16; ++i) { (*goal_id)[i] = info.goal_id.uuid[i]; diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 82a6dbda60..b30b6551a7 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include #include @@ -32,16 +32,16 @@ #include +#include #include #include #include -#include #include -#include +#include -#include "rclcpp_action/exceptions.hpp" -#include "rclcpp_action/create_client.hpp" #include "rclcpp_action/client.hpp" +#include "rclcpp_action/create_client.hpp" +#include "rclcpp_action/exceptions.hpp" #include "rclcpp_action/qos.hpp" #include "rclcpp_action/types.hpp" @@ -80,15 +80,11 @@ class TestClient : public ::testing::Test server_node = std::make_shared(server_node_name, namespace_name); char * goal_service_name = nullptr; - rcl_ret_t ret = rcl_action_get_goal_service_name( - action_name, allocator, &goal_service_name); + rcl_ret_t ret = rcl_action_get_goal_service_name(action_name, allocator, &goal_service_name); ASSERT_EQ(RCL_RET_OK, ret); goal_service = server_node->create_service( goal_service_name, - [this]( - const ActionGoalRequest::SharedPtr request, - ActionGoalResponse::SharedPtr response) - { + [this](const ActionGoalRequest::SharedPtr request, ActionGoalResponse::SharedPtr response) { response->stamp = clock.now(); response->accepted = (request->goal.order >= 0); if (response->accepted) { @@ -99,15 +95,13 @@ class TestClient : public ::testing::Test allocator.deallocate(goal_service_name, allocator.state); char * result_service_name = nullptr; - ret = rcl_action_get_result_service_name( - action_name, allocator, &result_service_name); + ret = rcl_action_get_result_service_name(action_name, allocator, &result_service_name); ASSERT_EQ(RCL_RET_OK, ret); result_service = server_node->create_service( result_service_name, [this]( const ActionGoalResultRequest::SharedPtr request, - ActionGoalResultResponse::SharedPtr response) - { + ActionGoalResultResponse::SharedPtr response) { if (goals.count(request->goal_id.uuid) == 1) { auto goal_request = goals[request->goal_id.uuid].first; auto goal_response = goals[request->goal_id.uuid].second; @@ -130,8 +124,7 @@ class TestClient : public ::testing::Test client_executor.spin_once(); for (int i = 1; i < goal_request->goal.order; ++i) { feedback_message.feedback.sequence.push_back( - feedback_message.feedback.sequence[i] + - feedback_message.feedback.sequence[i - 1]); + feedback_message.feedback.sequence[i] + feedback_message.feedback.sequence[i - 1]); feedback_publisher->publish(feedback_message); client_executor.spin_once(); } @@ -151,30 +144,27 @@ class TestClient : public ::testing::Test allocator.deallocate(result_service_name, allocator.state); char * cancel_service_name = nullptr; - ret = rcl_action_get_cancel_service_name( - action_name, allocator, &cancel_service_name); + ret = rcl_action_get_cancel_service_name(action_name, allocator, &cancel_service_name); ASSERT_EQ(RCL_RET_OK, ret); cancel_service = server_node->create_service( cancel_service_name, [this]( const ActionCancelGoalRequest::SharedPtr request, - ActionCancelGoalResponse::SharedPtr response) - { + ActionCancelGoalResponse::SharedPtr response) { rclcpp_action::GoalUUID zero_uuid; std::fill(zero_uuid.begin(), zero_uuid.end(), 0u); const rclcpp::Time cancel_stamp = request->goal_info.stamp; - bool cancel_all = ( - request->goal_info.goal_id.uuid == zero_uuid && - cancel_stamp == zero_stamp); + bool cancel_all = + (request->goal_info.goal_id.uuid == zero_uuid && cancel_stamp == zero_stamp); ActionStatusMessage status_message; auto it = goals.begin(); while (it != goals.end()) { auto goal_request = it->second.first; auto goal_response = it->second.second; const rclcpp::Time goal_stamp = goal_response->stamp; - bool cancel_this = ( - request->goal_info.goal_id.uuid == goal_request->goal_id.uuid || - cancel_stamp > goal_stamp); + bool cancel_this = + (request->goal_info.goal_id.uuid == goal_request->goal_id.uuid || + cancel_stamp > goal_stamp); if (cancel_all || cancel_this) { rclcpp_action::GoalStatus goal_status; goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid; @@ -194,8 +184,7 @@ class TestClient : public ::testing::Test allocator.deallocate(cancel_service_name, allocator.state); char * feedback_topic_name = nullptr; - ret = rcl_action_get_feedback_topic_name( - action_name, allocator, &feedback_topic_name); + ret = rcl_action_get_feedback_topic_name(action_name, allocator, &feedback_topic_name); ASSERT_EQ(RCL_RET_OK, ret); feedback_publisher = server_node->create_publisher(feedback_topic_name, 10); @@ -203,8 +192,7 @@ class TestClient : public ::testing::Test allocator.deallocate(feedback_topic_name, allocator.state); char * status_topic_name = nullptr; - ret = rcl_action_get_status_topic_name( - action_name, allocator, &status_topic_name); + ret = rcl_action_get_status_topic_name(action_name, allocator, &status_topic_name); ASSERT_EQ(RCL_RET_OK, ret); status_publisher = server_node->create_publisher( status_topic_name, rclcpp_action::DefaultActionStatusQoS()); @@ -255,9 +243,8 @@ class TestClient : public ::testing::Test std::map< rclcpp_action::GoalUUID, - std::pair< - typename ActionGoalRequest::SharedPtr, - typename ActionGoalResponse::SharedPtr>> goals; + std::pair> + goals; typename rclcpp::Service::SharedPtr goal_service; typename rclcpp::Service::SharedPtr result_service; typename rclcpp::Service::SharedPtr cancel_service; @@ -325,9 +312,8 @@ TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result) bool goal_response_received = false; auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.goal_response_callback = - [&goal_response_received] - (std::shared_future future) mutable - { + [&goal_response_received]( + std::shared_future future) mutable { auto goal_handle = future.get(); if (goal_handle) { goal_response_received = true; @@ -360,9 +346,8 @@ TEST_F(TestClient, async_send_goal_with_feedback_callback_wait_for_result) auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.feedback_callback = [&feedback_count]( - typename ActionGoalHandle::SharedPtr goal_handle, - const std::shared_ptr feedback) mutable - { + typename ActionGoalHandle::SharedPtr goal_handle, + const std::shared_ptr feedback) mutable { (void)goal_handle; (void)feedback; feedback_count++; @@ -393,13 +378,10 @@ TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result) bool result_callback_received = false; auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.result_callback = - [&result_callback_received]( - const typename ActionGoalHandle::WrappedResult & result) mutable - { + [&result_callback_received](const typename ActionGoalHandle::WrappedResult & result) mutable { if ( rclcpp_action::ResultCode::SUCCEEDED == result.code && - result.result->sequence.size() == 5u) - { + result.result->sequence.size() == 5u) { result_callback_received = true; } }; @@ -435,13 +417,10 @@ TEST_F(TestClient, async_get_result_with_callback) bool result_callback_received = false; auto future_result = action_client->async_get_result( goal_handle, - [&result_callback_received]( - const typename ActionGoalHandle::WrappedResult & result) mutable - { + [&result_callback_received](const typename ActionGoalHandle::WrappedResult & result) mutable { if ( rclcpp_action::ResultCode::SUCCEEDED == result.code && - result.result->sequence.size() == 5u) - { + result.result->sequence.size() == 5u) { result_callback_received = true; } }); @@ -486,14 +465,11 @@ TEST_F(TestClient, async_cancel_one_goal_with_callback) bool cancel_response_received = false; auto future_cancel = action_client->async_cancel_goal( goal_handle, - [&cancel_response_received, goal_handle]( - ActionCancelGoalResponse::SharedPtr response) mutable - { + [&cancel_response_received, goal_handle](ActionCancelGoalResponse::SharedPtr response) mutable { if ( ActionCancelGoalResponse::ERROR_NONE == response->return_code && 1ul == response->goals_canceling.size() && - goal_handle->get_goal_id() == response->goals_canceling[0].goal_id.uuid) - { + goal_handle->get_goal_id() == response->goals_canceling[0].goal_id.uuid) { cancel_response_received = true; } }); @@ -566,16 +542,13 @@ TEST_F(TestClient, async_cancel_all_goals_with_callback) ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); bool cancel_callback_received = false; - auto future_cancel_all = action_client->async_cancel_all_goals( - [&cancel_callback_received, goal_handle0, goal_handle1]( - ActionCancelGoalResponse::SharedPtr response) - { + auto future_cancel_all = + action_client->async_cancel_all_goals([&cancel_callback_received, goal_handle0, goal_handle1]( + ActionCancelGoalResponse::SharedPtr response) { if ( - response && - 2ul == response->goals_canceling.size() && + response && 2ul == response->goals_canceling.size() && goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid && - goal_handle1->get_goal_id() == response->goals_canceling[1].goal_id.uuid) - { + goal_handle1->get_goal_id() == response->goals_canceling[1].goal_id.uuid) { cancel_callback_received = true; } }); @@ -645,13 +618,10 @@ TEST_F(TestClient, async_cancel_some_goals_with_callback) bool cancel_callback_received = false; auto future_cancel_some = action_client->async_cancel_goals_before( goal_handle1->get_goal_stamp(), - [&cancel_callback_received, goal_handle0](ActionCancelGoalResponse::SharedPtr response) - { + [&cancel_callback_received, goal_handle0](ActionCancelGoalResponse::SharedPtr response) { if ( - response && - 1ul == response->goals_canceling.size() && - goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid) - { + response && 1ul == response->goals_canceling.size() && + goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid) { cancel_callback_received = true; } }); diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 35e2c4e032..861e402ee0 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -37,11 +37,11 @@ class TestServer : public ::testing::Test rclcpp::init(0, nullptr); } - std::shared_ptr - send_goal_request(rclcpp::Node::SharedPtr node, GoalUUID uuid) + std::shared_ptr send_goal_request( + rclcpp::Node::SharedPtr node, GoalUUID uuid) { - auto client = node->create_client( - "fibonacci/_action/send_goal"); + auto client = + node->create_client("fibonacci/_action/send_goal"); if (!client->wait_for_service(std::chrono::seconds(20))) { throw std::runtime_error("send goal service didn't become available"); } @@ -50,18 +50,16 @@ class TestServer : public ::testing::Test auto future = client->async_send_request(request); if ( rclcpp::executor::FutureReturnCode::SUCCESS != - rclcpp::spin_until_future_complete(node, future)) - { + rclcpp::spin_until_future_complete(node, future)) { throw std::runtime_error("send goal future didn't complete succesfully"); } return request; } - CancelResponse::SharedPtr - send_cancel_request(rclcpp::Node::SharedPtr node, GoalUUID uuid) + CancelResponse::SharedPtr send_cancel_request(rclcpp::Node::SharedPtr node, GoalUUID uuid) { - auto cancel_client = node->create_client( - "fibonacci/_action/cancel_goal"); + auto cancel_client = + node->create_client("fibonacci/_action/cancel_goal"); if (!cancel_client->wait_for_service(std::chrono::seconds(20))) { throw std::runtime_error("cancel goal service didn't become available"); } @@ -70,8 +68,7 @@ class TestServer : public ::testing::Test auto future = cancel_client->async_send_request(request); if ( rclcpp::executor::FutureReturnCode::SUCCESS != - rclcpp::spin_until_future_complete(node, future)) - { + rclcpp::spin_until_future_complete(node, future)) { throw std::runtime_error("cancel goal future didn't complete succesfully"); } return future.get(); @@ -84,13 +81,12 @@ TEST_F(TestServer, construction_and_destruction) using GoalHandle = rclcpp_action::ServerGoalHandle; auto as = rclcpp_action::create_server( - node, "fibonacci", + node, + "fibonacci", [](const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::REJECT; }, - [](std::shared_ptr) { - return rclcpp_action::CancelResponse::REJECT; - }, + [](std::shared_ptr) { return rclcpp_action::CancelResponse::REJECT; }, [](std::shared_ptr) {}); (void)as; } @@ -101,27 +97,25 @@ TEST_F(TestServer, handle_goal_called) GoalUUID received_uuid; auto handle_goal = [&received_uuid]( - const GoalUUID & uuid, std::shared_ptr) - { - received_uuid = uuid; - return rclcpp_action::GoalResponse::REJECT; - }; + const GoalUUID & uuid, std::shared_ptr) { + received_uuid = uuid; + return rclcpp_action::GoalResponse::REJECT; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; auto as = rclcpp_action::create_server( - node, "fibonacci", + node, + "fibonacci", handle_goal, - [](std::shared_ptr) { - return rclcpp_action::CancelResponse::REJECT; - }, + [](std::shared_ptr) { return rclcpp_action::CancelResponse::REJECT; }, [](std::shared_ptr) {}); (void)as; // Create a client that calls the goal request service // Make sure the UUID received is the same as the one sent - auto client = node->create_client( - "fibonacci/_action/send_goal"); + auto client = + node->create_client("fibonacci/_action/send_goal"); ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); @@ -132,8 +126,7 @@ TEST_F(TestServer, handle_goal_called) auto future = client->async_send_request(request); ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); + rclcpp::executor::FutureReturnCode::SUCCESS, rclcpp::spin_until_future_complete(node, future)); ASSERT_EQ(uuid, received_uuid); } @@ -143,26 +136,22 @@ TEST_F(TestServer, handle_accepted_called) auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_accepted"); const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", + node, + "fibonacci", handle_goal, - [](std::shared_ptr) { - return rclcpp_action::CancelResponse::REJECT; - }, + [](std::shared_ptr) { return rclcpp_action::CancelResponse::REJECT; }, handle_accepted); (void)as; @@ -179,30 +168,23 @@ TEST_F(TestServer, handle_cancel_called) auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::ACCEPT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; send_goal_request(node, uuid); @@ -220,30 +202,23 @@ TEST_F(TestServer, handle_cancel_reject) auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::REJECT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; send_goal_request(node, uuid); @@ -263,30 +238,23 @@ TEST_F(TestServer, handle_cancel_unknown_goal) const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; const GoalUUID unknown_uuid{{11, 22, 33, 44, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::ACCEPT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; send_goal_request(node, uuid); @@ -305,31 +273,24 @@ TEST_F(TestServer, handle_cancel_terminated_goal) auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::ACCEPT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - handle->succeed(std::make_shared()); - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + handle->succeed(std::make_shared()); + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; send_goal_request(node, uuid); @@ -348,38 +309,31 @@ TEST_F(TestServer, publish_status_accepted) auto node = std::make_shared("status_accept_node", "/rclcpp_action/status_accept"); const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::REJECT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) - { + "fibonacci/_action/status", + 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) { received_msgs.push_back(list); }); @@ -411,38 +365,31 @@ TEST_F(TestServer, publish_status_canceling) auto node = std::make_shared("status_cancel_node", "/rclcpp_action/status_cancel"); const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::ACCEPT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) - { + "fibonacci/_action/status", + 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) { received_msgs.push_back(list); }); @@ -468,38 +415,31 @@ TEST_F(TestServer, publish_status_canceled) auto node = std::make_shared("status_canceled", "/rclcpp_action/status_canceled"); const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::ACCEPT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) - { + "fibonacci/_action/status", + 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) { received_msgs.push_back(list); }); @@ -527,38 +467,31 @@ TEST_F(TestServer, publish_status_succeeded) auto node = std::make_shared("status_succeeded", "/rclcpp_action/status_succeeded"); const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::REJECT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) - { + "fibonacci/_action/status", + 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) { received_msgs.push_back(list); }); @@ -584,38 +517,31 @@ TEST_F(TestServer, publish_status_aborted) auto node = std::make_shared("status_aborted", "/rclcpp_action/status_aborted"); const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::REJECT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) - { + "fibonacci/_action/status", + 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) { received_msgs.push_back(list); }); @@ -641,38 +567,30 @@ TEST_F(TestServer, publish_feedback) auto node = std::make_shared("pub_feedback", "/rclcpp_action/pub_feedback"); const GoalUUID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::REJECT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; // Subscribe to feedback messages using FeedbackT = Fibonacci::Impl::FeedbackMessage; std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::SharedPtr msg) - { + "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::SharedPtr msg) { received_msgs.push_back(msg); }); @@ -699,37 +617,30 @@ TEST_F(TestServer, get_result) auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::REJECT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; send_goal_request(node, uuid); // Send result request - auto result_client = node->create_client( - "fibonacci/_action/get_result"); + auto result_client = + node->create_client("fibonacci/_action/get_result"); if (!result_client->wait_for_service(std::chrono::seconds(20))) { throw std::runtime_error("get result service didn't become available"); } @@ -744,8 +655,7 @@ TEST_F(TestServer, get_result) // Wait for the result request to be received ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); + rclcpp::executor::FutureReturnCode::SUCCESS, rclcpp::spin_until_future_complete(node, future)); auto response = future.get(); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); @@ -757,30 +667,23 @@ TEST_F(TestServer, deferred_execution) auto node = std::make_shared("defer_exec", "/rclcpp_action/defer_exec"); const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; - auto handle_goal = []( - const GoalUUID &, std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; - }; + auto handle_goal = [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }; using GoalHandle = rclcpp_action::ServerGoalHandle; - auto handle_cancel = [](std::shared_ptr) - { - return rclcpp_action::CancelResponse::REJECT; - }; + auto handle_cancel = [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }; std::shared_ptr received_handle; - auto handle_accepted = [&received_handle](std::shared_ptr handle) - { - received_handle = handle; - }; + auto handle_accepted = [&received_handle](std::shared_ptr handle) { + received_handle = handle; + }; auto as = rclcpp_action::create_server( - node, "fibonacci", - handle_goal, - handle_cancel, - handle_accepted); + node, "fibonacci", handle_goal, handle_cancel, handle_accepted); (void)as; send_goal_request(node, uuid); diff --git a/rclcpp_components/include/rclcpp_components/node_factory.hpp b/rclcpp_components/include/rclcpp_components/node_factory.hpp index 67e6cd7331..5363be8966 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory.hpp @@ -37,9 +37,7 @@ class NodeFactory /** * \param[in] options Additional options used in the construction of the component. */ - virtual - NodeInstanceWrapper - create_node_instance(const rclcpp::NodeOptions & options) = 0; + virtual NodeInstanceWrapper create_node_instance(const rclcpp::NodeOptions & options) = 0; }; } // namespace rclcpp_components diff --git a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp index 988b8036bb..25bc9d26b3 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp @@ -39,13 +39,11 @@ class NodeFactoryTemplate : public NodeFactory /** * \param[in] options Additional options used in the construction of the component. */ - NodeInstanceWrapper - create_node_instance(const rclcpp::NodeOptions & options) override + NodeInstanceWrapper create_node_instance(const rclcpp::NodeOptions & options) override { auto node = std::make_shared(options); - return NodeInstanceWrapper( - node, std::bind(&NodeT::get_node_base_interface, node)); + return NodeInstanceWrapper(node, std::bind(&NodeT::get_node_base_interface, node)); } }; } // namespace rclcpp_components diff --git a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp index 2705e4e8b3..1c4dc3064a 100644 --- a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp +++ b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp @@ -26,18 +26,19 @@ namespace rclcpp_components class NodeInstanceWrapper { public: - using NodeBaseInterfaceGetter = std::function< - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr(const std::shared_ptr &)>; + using NodeBaseInterfaceGetter = + std::function &)>; - NodeInstanceWrapper() - : node_instance_(nullptr) - {} + NodeInstanceWrapper() : node_instance_(nullptr) + { + } NodeInstanceWrapper( - std::shared_ptr node_instance, - NodeBaseInterfaceGetter node_base_interface_getter) + std::shared_ptr node_instance, NodeBaseInterfaceGetter node_base_interface_getter) : node_instance_(node_instance), node_base_interface_getter_(node_base_interface_getter) - {} + { + } /// Get a type-erased pointer to the original Node instance /** @@ -46,8 +47,7 @@ class NodeInstanceWrapper * * \return Shared pointer to the encapsulated Node instance. */ - const std::shared_ptr - get_node_instance() const + const std::shared_ptr get_node_instance() const { return node_instance_; } @@ -56,8 +56,7 @@ class NodeInstanceWrapper /** * \return Shared NodeBaseInterface pointer of the encapsulated Node instance. */ - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_base_interface() + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() { return node_base_interface_getter_(node_instance_); } diff --git a/rclcpp_components/include/rclcpp_components/register_node_macro.hpp b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp index 340d8021bb..ae55861f2b 100644 --- a/rclcpp_components/include/rclcpp_components/register_node_macro.hpp +++ b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp @@ -31,8 +31,7 @@ * Note: NodeClass does not need to inherit from `rclcpp::Node`, but it is the easiest way. */ #define RCLCPP_COMPONENTS_REGISTER_NODE(NodeClass) \ - CLASS_LOADER_REGISTER_CLASS( \ - rclcpp_components::NodeFactoryTemplate, \ - rclcpp_components::NodeFactory) + CLASS_LOADER_REGISTER_CLASS( \ + rclcpp_components::NodeFactoryTemplate, rclcpp_components::NodeFactory) #endif // RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__ diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index edd20fdb9f..3ebb22a3e7 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -28,20 +28,15 @@ using namespace std::placeholders; namespace rclcpp_components { -ComponentManager::ComponentManager( - std::weak_ptr executor) -: Node("ComponentManager"), - executor_(executor) +ComponentManager::ComponentManager(std::weak_ptr executor) +: Node("ComponentManager"), executor_(executor) { loadNode_srv_ = create_service( - "~/_container/load_node", - std::bind(&ComponentManager::OnLoadNode, this, _1, _2, _3)); + "~/_container/load_node", std::bind(&ComponentManager::OnLoadNode, this, _1, _2, _3)); unloadNode_srv_ = create_service( - "~/_container/unload_node", - std::bind(&ComponentManager::OnUnloadNode, this, _1, _2, _3)); + "~/_container/unload_node", std::bind(&ComponentManager::OnUnloadNode, this, _1, _2, _3)); listNodes_srv_ = create_service( - "~/_container/list_nodes", - std::bind(&ComponentManager::OnListNodes, this, _1, _2, _3)); + "~/_container/list_nodes", std::bind(&ComponentManager::OnListNodes, this, _1, _2, _3)); } ComponentManager::~ComponentManager() @@ -56,15 +51,12 @@ ComponentManager::~ComponentManager() } } -std::vector -ComponentManager::get_component_resources(const std::string & package_name) const +std::vector ComponentManager::get_component_resources( + const std::string & package_name) const { std::string content; std::string base_path; - if ( - !ament_index_cpp::get_resource( - "rclcpp_components", package_name, content, &base_path)) - { + if (!ament_index_cpp::get_resource("rclcpp_components", package_name, content, &base_path)) { throw ComponentManagerException("Could not find requested resource in ament index"); } @@ -85,8 +77,8 @@ ComponentManager::get_component_resources(const std::string & package_name) cons return resources; } -std::shared_ptr -ComponentManager::create_component_factory(const ComponentResource & resource) +std::shared_ptr ComponentManager::create_component_factory( + const ComponentResource & resource) { std::string library_path = resource.second; std::string class_name = resource.first; @@ -116,13 +108,12 @@ ComponentManager::create_component_factory(const ComponentResource & resource) return {}; } -void -ComponentManager::OnLoadNode( +void ComponentManager::OnLoadNode( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) { - (void) request_header; + (void)request_header; try { auto resources = get_component_resources(request->package_name); @@ -161,9 +152,9 @@ ComponentManager::OnLoadNode( } auto options = rclcpp::NodeOptions() - .use_global_arguments(false) - .parameter_overrides(parameters) - .arguments(remap_rules); + .use_global_arguments(false) + .parameter_overrides(parameters) + .arguments(remap_rules); auto node_id = unique_id++; @@ -196,7 +187,8 @@ ComponentManager::OnLoadNode( return; } RCLCPP_ERROR( - get_logger(), "Failed to find class with the requested plugin name '%s' in " + get_logger(), + "Failed to find class with the requested plugin name '%s' in " "the loaded library", request->plugin_name.c_str()); response->error_message = "Failed to find class with the requested plugin name."; @@ -208,13 +200,12 @@ ComponentManager::OnLoadNode( } } -void -ComponentManager::OnUnloadNode( +void ComponentManager::OnUnloadNode( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) { - (void) request_header; + (void)request_header; auto wrapper = node_wrappers_.find(request->unique_id); @@ -233,14 +224,13 @@ ComponentManager::OnUnloadNode( } } -void -ComponentManager::OnListNodes( +void ComponentManager::OnListNodes( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) { - (void) request_header; - (void) request; + (void)request_header; + (void)request; for (auto & wrapper : node_wrappers_) { response->unique_ids.push_back(wrapper.first); diff --git a/rclcpp_components/src/component_manager.hpp b/rclcpp_components/src/component_manager.hpp index 62d7e1e85d..f3c20a1b58 100644 --- a/rclcpp_components/src/component_manager.hpp +++ b/rclcpp_components/src/component_manager.hpp @@ -27,9 +27,9 @@ #include "rclcpp/node_options.hpp" #include "rclcpp/rclcpp.hpp" +#include "composition_interfaces/srv/list_nodes.hpp" #include "composition_interfaces/srv/load_node.hpp" #include "composition_interfaces/srv/unload_node.hpp" -#include "composition_interfaces/srv/list_nodes.hpp" #include "rclcpp_components/node_factory.hpp" @@ -40,7 +40,9 @@ class ComponentManagerException : public std::runtime_error { public: explicit ComponentManagerException(const std::string & error_desc) - : std::runtime_error(error_desc) {} + : std::runtime_error(error_desc) + { + } }; class ComponentManager : public rclcpp::Node @@ -56,33 +58,28 @@ class ComponentManager : public rclcpp::Node */ using ComponentResource = std::pair; - ComponentManager( - std::weak_ptr executor); + ComponentManager(std::weak_ptr executor); ~ComponentManager(); /// Return a list of valid loadable components in a given package. - std::vector - get_component_resources(const std::string & package_name) const; + std::vector get_component_resources(const std::string & package_name) const; - std::shared_ptr - create_component_factory(const ComponentResource & resource); + std::shared_ptr create_component_factory( + const ComponentResource & resource); private: - void - OnLoadNode( + void OnLoadNode( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response); - void - OnUnloadNode( + void OnUnloadNode( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response); - void - OnListNodes( + void OnListNodes( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response); @@ -90,7 +87,7 @@ class ComponentManager : public rclcpp::Node private: std::weak_ptr executor_; - uint64_t unique_id {1}; + uint64_t unique_id{1}; std::map> loaders_; std::map node_wrappers_; diff --git a/rclcpp_components/test/components/test_component.cpp b/rclcpp_components/test/components/test_component.cpp index dc85187650..e1220ed94f 100644 --- a/rclcpp_components/test/components/test_component.cpp +++ b/rclcpp_components/test/components/test_component.cpp @@ -45,8 +45,7 @@ class TestComponentNoNode { } - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_base_interface() + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() { return node_.get_node_base_interface(); } @@ -55,7 +54,6 @@ class TestComponentNoNode rclcpp::Node node_; }; - } // namespace test_rclcpp_components #include "rclcpp_components/register_node_macro.hpp" diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index 3e267b531a..e53a47e4a3 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -17,9 +17,9 @@ #include #include +#include "composition_interfaces/srv/list_nodes.hpp" #include "composition_interfaces/srv/load_node.hpp" #include "composition_interfaces/srv/unload_node.hpp" -#include "composition_interfaces/srv/list_nodes.hpp" #include "component_manager.hpp" @@ -114,8 +114,8 @@ TEST_F(TestComponentManager, load_components) auto node_names = node->get_node_names(); auto find_in_nodes = [node_names](std::string name) { - return std::find(node_names.begin(), node_names.end(), name) != node_names.end(); - }; + return std::find(node_names.begin(), node_names.end(), name) != node_names.end(); + }; EXPECT_TRUE(find_in_nodes("/test_component_foo")); EXPECT_TRUE(find_in_nodes("/test_component_bar")); @@ -170,7 +170,6 @@ TEST_F(TestComponentManager, load_invalid_components) } } - TEST_F(TestComponentManager, list_components) { auto exec = std::make_shared(); @@ -261,8 +260,8 @@ TEST_F(TestComponentManager, unload_component) auto node_names = node->get_node_names(); auto find_in_nodes = [node_names](std::string name) { - return std::find(node_names.begin(), node_names.end(), name) != node_names.end(); - }; + return std::find(node_names.begin(), node_names.end(), name) != node_names.end(); + }; EXPECT_TRUE(find_in_nodes("/test_component_foo")); { diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index fec67c7871..dd7aa30bdf 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -37,7 +37,6 @@ #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" -#include "rclcpp/node_options.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" @@ -48,6 +47,7 @@ #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_waitables_interface.hpp" +#include "rclcpp/node_options.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/publisher_options.hpp" @@ -58,8 +58,8 @@ #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/transition.hpp" #include "rclcpp_lifecycle/visibility_control.h" @@ -77,15 +77,13 @@ template using SubscriptionOptionsWithAllocator = rclcpp::SubscriptionOptionsWithAllocator; template -PublisherOptionsWithAllocator -create_default_publisher_options() +PublisherOptionsWithAllocator create_default_publisher_options() { return rclcpp::PublisherOptionsWithAllocator(); } template -SubscriptionOptionsWithAllocator -create_default_subscription_options() +SubscriptionOptionsWithAllocator create_default_subscription_options() { return rclcpp::SubscriptionOptionsWithAllocator(); } @@ -95,7 +93,7 @@ create_default_subscription_options() * has lifecycle nodeinterface for configuring this node. */ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, - public std::enable_shared_from_this + public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_DEFINITIONS(LifecycleNode) @@ -108,8 +106,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC explicit LifecycleNode( - const std::string & node_name, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /// Create a node based on the node name and a rclcpp::Context. /** @@ -129,30 +126,26 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, /// Get the name of the node. // \return The name of the node. RCLCPP_LIFECYCLE_PUBLIC - const char * - get_name() const; + const char * get_name() const; /// Get the namespace of the node. // \return The namespace of the node. RCLCPP_LIFECYCLE_PUBLIC - const char * - get_namespace() const; + const char * get_namespace() const; /// Get the logger of the node. /** \return The logger of the node. */ RCLCPP_LIFECYCLE_PUBLIC - rclcpp::Logger - get_logger() const; + rclcpp::Logger get_logger() const; /// Create and return a callback group. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + rclcpp::callback_group::CallbackGroup::SharedPtr create_callback_group( + rclcpp::callback_group::CallbackGroupType group_type); /// Return the list of callback groups in the node. RCLCPP_LIFECYCLE_PUBLIC - const std::vector & - get_callback_groups() const; + const std::vector & get_callback_groups() const; /// Create and return a Publisher. /** @@ -162,13 +155,11 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * \return Shared pointer to the created lifecycle publisher. */ template> - std::shared_ptr> - create_publisher( + std::shared_ptr> create_publisher( const std::string & topic_name, const rclcpp::QoS & qos, const PublisherOptionsWithAllocator & options = - create_default_publisher_options() - ); + create_default_publisher_options()); /// Create and return a Subscription. /** @@ -188,17 +179,15 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, typename CallbackT, typename AllocatorT = std::allocator, typename SubscriptionT = rclcpp::Subscription> - std::shared_ptr - create_subscription( + std::shared_ptr create_subscription( const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, const SubscriptionOptionsWithAllocator & options = - create_default_subscription_options(), + create_default_subscription_options(), typename rclcpp::message_memory_strategy::MessageMemoryStrategy< - typename rclcpp::subscription_traits::has_message_type::type, AllocatorT - >::SharedPtr - msg_mem_strat = nullptr); + typename rclcpp::subscription_traits::has_message_type::type, + AllocatorT>::SharedPtr msg_mem_strat = nullptr); /// Create a timer. /** @@ -207,24 +196,21 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * \param[in] group Callback group to execute this timer's callback in. */ template - typename rclcpp::WallTimer::SharedPtr - create_wall_timer( + typename rclcpp::WallTimer::SharedPtr create_wall_timer( std::chrono::duration period, CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Client. */ template - typename rclcpp::Client::SharedPtr - create_client( + typename rclcpp::Client::SharedPtr create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Service. */ template - typename rclcpp::Service::SharedPtr - create_service( + typename rclcpp::Service::SharedPtr create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, @@ -235,174 +221,150 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * \sa rclcpp::Node::declare_parameter */ RCLCPP_LIFECYCLE_PUBLIC - const rclcpp::ParameterValue & - declare_parameter( + const rclcpp::ParameterValue & declare_parameter( const std::string & name, const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(), const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor()); + rcl_interfaces::msg::ParameterDescriptor()); /// Declare and initialize a parameter with a type. /** * \sa rclcpp::Node::declare_parameter */ template - auto - declare_parameter( + auto declare_parameter( const std::string & name, const ParameterT & default_value, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor()); + rcl_interfaces::msg::ParameterDescriptor()); /// Declare and initialize several parameters with the same namespace and type. /** * \sa rclcpp::Node::declare_parameters */ template - std::vector - declare_parameters( - const std::string & namespace_, - const std::map & parameters); + std::vector declare_parameters( + const std::string & namespace_, const std::map & parameters); /// Declare and initialize several parameters with the same namespace and type. /** * \sa rclcpp::Node::declare_parameters */ template - std::vector - declare_parameters( + std::vector declare_parameters( const std::string & namespace_, - const std::map< - std::string, - std::pair - > & parameters); + const std::map> & + parameters); /// Undeclare a previously declared parameter. /** * \sa rclcpp::Node::undeclare_parameter */ RCLCPP_LIFECYCLE_PUBLIC - void - undeclare_parameter(const std::string & name); + void undeclare_parameter(const std::string & name); /// Return true if a given parameter is declared. /** * \sa rclcpp::Node::has_parameter */ RCLCPP_LIFECYCLE_PUBLIC - bool - has_parameter(const std::string & name) const; + bool has_parameter(const std::string & name) const; /// Set a single parameter. /** * \sa rclcpp::Node::set_parameter */ RCLCPP_LIFECYCLE_PUBLIC - rcl_interfaces::msg::SetParametersResult - set_parameter(const rclcpp::Parameter & parameter); + rcl_interfaces::msg::SetParametersResult set_parameter(const rclcpp::Parameter & parameter); /// Set one or more parameters, one at a time. /** * \sa rclcpp::Node::set_parameters */ RCLCPP_LIFECYCLE_PUBLIC - std::vector - set_parameters(const std::vector & parameters); + std::vector set_parameters( + const std::vector & parameters); /// Set one or more parameters, all at once. /** * \sa rclcpp::Node::set_parameters_atomically */ RCLCPP_LIFECYCLE_PUBLIC - rcl_interfaces::msg::SetParametersResult - set_parameters_atomically(const std::vector & parameters); + rcl_interfaces::msg::SetParametersResult set_parameters_atomically( + const std::vector & parameters); /// Return the parameter by the given name. /** * \sa rclcpp::Node::get_parameter */ RCLCPP_LIFECYCLE_PUBLIC - rclcpp::Parameter - get_parameter(const std::string & name) const; + rclcpp::Parameter get_parameter(const std::string & name) const; /// Get the value of a parameter by the given name, and return true if it was set. /** * \sa rclcpp::Node::get_parameter */ RCLCPP_LIFECYCLE_PUBLIC - bool - get_parameter( - const std::string & name, - rclcpp::Parameter & parameter) const; + bool get_parameter(const std::string & name, rclcpp::Parameter & parameter) const; /// Get the value of a parameter by the given name, and return true if it was set. /** * \sa rclcpp::Node::get_parameter */ template - bool - get_parameter(const std::string & name, ParameterT & parameter) const; + bool get_parameter(const std::string & name, ParameterT & parameter) const; /// Get the parameter value, or the "alternative_value" if not set, and assign it to "parameter". /** * \sa rclcpp::Node::get_parameter_or */ template - bool - get_parameter_or( - const std::string & name, - ParameterT & value, - const ParameterT & alternative_value) const; + bool get_parameter_or( + const std::string & name, ParameterT & value, const ParameterT & alternative_value) const; /// Return the parameters by the given parameter names. /** * \sa rclcpp::Node::get_parameters */ RCLCPP_LIFECYCLE_PUBLIC - std::vector - get_parameters(const std::vector & names) const; + std::vector get_parameters(const std::vector & names) const; /// Get the parameter values for all parameters that have a given prefix. /** * \sa rclcpp::Node::get_parameters */ template - bool - get_parameters( - const std::string & prefix, - std::map & values) const; + bool get_parameters(const std::string & prefix, std::map & values) const; /// Return the parameter descriptor for the given parameter name. /** * \sa rclcpp::Node::describe_parameter */ RCLCPP_LIFECYCLE_PUBLIC - rcl_interfaces::msg::ParameterDescriptor - describe_parameter(const std::string & name) const; + rcl_interfaces::msg::ParameterDescriptor describe_parameter(const std::string & name) const; /// Return a vector of parameter descriptors, one for each of the given names. /** * \sa rclcpp::Node::describe_parameters */ RCLCPP_LIFECYCLE_PUBLIC - std::vector - describe_parameters(const std::vector & names) const; + std::vector describe_parameters( + const std::vector & names) const; /// Return a vector of parameter types, one for each of the given names. /** * \sa rclcpp::Node::get_parameter_types */ RCLCPP_LIFECYCLE_PUBLIC - std::vector - get_parameter_types(const std::vector & names) const; + std::vector get_parameter_types(const std::vector & names) const; /// Return a list of parameters with any of the given prefixes, up to the given depth. /** * \sa rclcpp::Node::list_parameters */ RCLCPP_LIFECYCLE_PUBLIC - rcl_interfaces::msg::ListParametersResult - list_parameters(const std::vector & prefixes, uint64_t depth) const; + rcl_interfaces::msg::ListParametersResult list_parameters( + const std::vector & prefixes, uint64_t depth) const; using OnParametersSetCallbackType = rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; @@ -412,29 +374,24 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * \sa rclcpp::Node::set_on_parameters_set_callback */ RCLCPP_LIFECYCLE_PUBLIC - rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType - set_on_parameters_set_callback( + rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType set_on_parameters_set_callback( rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType callback); RCLCPP_LIFECYCLE_PUBLIC - std::vector - get_node_names() const; + std::vector get_node_names() const; RCLCPP_LIFECYCLE_PUBLIC - std::map> - get_topic_names_and_types(bool no_demangle = false) const; + std::map> get_topic_names_and_types( + bool no_demangle = false) const; RCLCPP_LIFECYCLE_PUBLIC - std::map> - get_service_names_and_types() const; + std::map> get_service_names_and_types() const; RCLCPP_LIFECYCLE_PUBLIC - size_t - count_publishers(const std::string & topic_name) const; + size_t count_publishers(const std::string & topic_name) const; RCLCPP_LIFECYCLE_PUBLIC - size_t - count_subscribers(const std::string & topic_name) const; + size_t count_subscribers(const std::string & topic_name) const; /// Return a graph event, which will be set anytime a graph change occurs. /* The graph Event object is a loan which must be returned. @@ -442,8 +399,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * out of scope. */ RCLCPP_LIFECYCLE_PUBLIC - rclcpp::Event::SharedPtr - get_graph_event(); + rclcpp::Event::SharedPtr get_graph_event(); /// Wait for a graph event to occur by waiting on an Event to become set. /* The given Event must be acquire through the get_graph_event() method. @@ -453,190 +409,153 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * get_graph_event(). */ RCLCPP_LIFECYCLE_PUBLIC - void - wait_for_graph_change( - rclcpp::Event::SharedPtr event, - std::chrono::nanoseconds timeout); + void wait_for_graph_change(rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout); RCLCPP_LIFECYCLE_PUBLIC - rclcpp::Clock::SharedPtr - get_clock(); + rclcpp::Clock::SharedPtr get_clock(); RCLCPP_LIFECYCLE_PUBLIC - rclcpp::Time - now(); + rclcpp::Time now(); /// Return the Node's internal NodeBaseInterface implementation. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_base_interface(); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); /// Return the Node's internal NodeClockInterface implementation. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::node_interfaces::NodeClockInterface::SharedPtr - get_node_clock_interface(); + rclcpp::node_interfaces::NodeClockInterface::SharedPtr get_node_clock_interface(); /// Return the Node's internal NodeGraphInterface implementation. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::node_interfaces::NodeGraphInterface::SharedPtr - get_node_graph_interface(); + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr get_node_graph_interface(); /// Return the Node's internal NodeLoggingInterface implementation. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr - get_node_logging_interface(); + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr get_node_logging_interface(); /// Return the Node's internal NodeTimersInterface implementation. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::node_interfaces::NodeTimersInterface::SharedPtr - get_node_timers_interface(); + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr get_node_timers_interface(); /// Return the Node's internal NodeTopicsInterface implementation. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr - get_node_topics_interface(); + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr get_node_topics_interface(); /// Return the Node's internal NodeServicesInterface implementation. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr - get_node_services_interface(); + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr get_node_services_interface(); /// Return the Node's internal NodeParametersInterface implementation. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr - get_node_parameters_interface(); + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface(); /// Return the Node's internal NodeParametersInterface implementation. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr - get_node_time_source_interface(); + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface(); /// Return the Node's internal NodeWaitablesInterface implementation. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr - get_node_waitables_interface(); + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr get_node_waitables_interface(); /// Return the NodeOptions used when creating this node. RCLCPP_LIFECYCLE_PUBLIC - const rclcpp::NodeOptions & - get_node_options() const; + const rclcpp::NodeOptions & get_node_options() const; // // LIFECYCLE COMPONENTS // RCLCPP_LIFECYCLE_PUBLIC - const State & - get_current_state(); + const State & get_current_state(); RCLCPP_LIFECYCLE_PUBLIC - std::vector - get_available_states(); + std::vector get_available_states(); RCLCPP_LIFECYCLE_PUBLIC - std::vector - get_available_transitions(); + std::vector get_available_transitions(); /// trigger the specified transition /* * return the new state after this transition */ RCLCPP_LIFECYCLE_PUBLIC - const State & - trigger_transition(const Transition & transition); + const State & trigger_transition(const Transition & transition); RCLCPP_LIFECYCLE_PUBLIC - const State & - trigger_transition( + const State & trigger_transition( const Transition & transition, LifecycleNodeInterface::CallbackReturn & cb_return_code); RCLCPP_LIFECYCLE_PUBLIC - const State & - trigger_transition(uint8_t transition_id); + const State & trigger_transition(uint8_t transition_id); RCLCPP_LIFECYCLE_PUBLIC - const State & - trigger_transition( + const State & trigger_transition( uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code); RCLCPP_LIFECYCLE_PUBLIC - const State & - configure(); + const State & configure(); RCLCPP_LIFECYCLE_PUBLIC - const State & - configure(LifecycleNodeInterface::CallbackReturn & cb_return_code); + const State & configure(LifecycleNodeInterface::CallbackReturn & cb_return_code); RCLCPP_LIFECYCLE_PUBLIC - const State & - cleanup(); + const State & cleanup(); RCLCPP_LIFECYCLE_PUBLIC - const State & - cleanup(LifecycleNodeInterface::CallbackReturn & cb_return_code); + const State & cleanup(LifecycleNodeInterface::CallbackReturn & cb_return_code); RCLCPP_LIFECYCLE_PUBLIC - const State & - activate(); + const State & activate(); RCLCPP_LIFECYCLE_PUBLIC - const State & - activate(LifecycleNodeInterface::CallbackReturn & cb_return_code); + const State & activate(LifecycleNodeInterface::CallbackReturn & cb_return_code); RCLCPP_LIFECYCLE_PUBLIC - const State & - deactivate(); + const State & deactivate(); RCLCPP_LIFECYCLE_PUBLIC - const State & - deactivate(LifecycleNodeInterface::CallbackReturn & cb_return_code); + const State & deactivate(LifecycleNodeInterface::CallbackReturn & cb_return_code); RCLCPP_LIFECYCLE_PUBLIC - const State & - shutdown(); + const State & shutdown(); RCLCPP_LIFECYCLE_PUBLIC - const State & - shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code); + const State & shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code); RCLCPP_LIFECYCLE_PUBLIC - bool - register_on_configure(std::function fcn); + bool register_on_configure( + std::function fcn); RCLCPP_LIFECYCLE_PUBLIC - bool - register_on_cleanup(std::function fcn); + bool register_on_cleanup( + std::function fcn); RCLCPP_LIFECYCLE_PUBLIC - bool - register_on_shutdown(std::function fcn); + bool register_on_shutdown( + std::function fcn); RCLCPP_LIFECYCLE_PUBLIC - bool - register_on_activate(std::function fcn); + bool register_on_activate( + std::function fcn); RCLCPP_LIFECYCLE_PUBLIC - bool - register_on_deactivate(std::function fcn); + bool register_on_deactivate( + std::function fcn); RCLCPP_LIFECYCLE_PUBLIC - bool - register_on_error(std::function fcn); + bool register_on_error(std::function fcn); protected: RCLCPP_LIFECYCLE_PUBLIC - void - add_publisher_handle(std::shared_ptr pub); + void add_publisher_handle(std::shared_ptr pub); RCLCPP_LIFECYCLE_PUBLIC - void - add_timer_handle(std::shared_ptr timer); + void add_timer_handle(std::shared_ptr timer); private: RCLCPP_DISABLE_COPY(LifecycleNode) RCLCPP_LIFECYCLE_PUBLIC - bool - group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group); + bool group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group); rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 68f573b3b8..e820dd454c 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -22,12 +22,12 @@ #include #include "rclcpp/contexts/default_context.hpp" -#include "rclcpp/intra_process_manager.hpp" -#include "rclcpp/event.hpp" -#include "rclcpp/parameter.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" #include "rclcpp/create_subscription.hpp" +#include "rclcpp/event.hpp" +#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/parameter.hpp" #include "rclcpp/subscription_options.hpp" #include "rclcpp/type_support_decl.hpp" @@ -48,54 +48,40 @@ LifecycleNode::create_publisher( { using PublisherT = rclcpp_lifecycle::LifecyclePublisher; return rclcpp::create_publisher( - *this, - topic_name, - qos, - options); + *this, topic_name, qos, options); } // TODO(karsten1987): Create LifecycleSubscriber -template< - typename MessageT, - typename CallbackT, - typename AllocatorT, - typename SubscriptionT> -std::shared_ptr -LifecycleNode::create_subscription( +template +std::shared_ptr LifecycleNode::create_subscription( const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< - typename rclcpp::subscription_traits::has_message_type::type, AllocatorT>::SharedPtr - msg_mem_strat) + typename rclcpp::subscription_traits::has_message_type::type, + AllocatorT>::SharedPtr msg_mem_strat) { return rclcpp::create_subscription( - *this, - topic_name, - qos, - std::forward(callback), - options, - msg_mem_strat); + *this, topic_name, qos, std::forward(callback), options, msg_mem_strat); } template -typename rclcpp::WallTimer::SharedPtr -LifecycleNode::create_wall_timer( +typename rclcpp::WallTimer::SharedPtr LifecycleNode::create_wall_timer( std::chrono::duration period, CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { auto timer = rclcpp::WallTimer::make_shared( std::chrono::duration_cast(period), - std::move(callback), this->node_base_->get_context()); + std::move(callback), + this->node_base_->get_context()); node_timers_->add_timer(timer, group); return timer; } template -typename rclcpp::Client::SharedPtr -LifecycleNode::create_client( +typename rclcpp::Client::SharedPtr LifecycleNode::create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile, rclcpp::callback_group::CallbackGroup::SharedPtr group) @@ -106,11 +92,7 @@ LifecycleNode::create_client( using rclcpp::Client; using rclcpp::ClientBase; - auto cli = Client::make_shared( - node_base_.get(), - node_graph_, - service_name, - options); + auto cli = Client::make_shared(node_base_.get(), node_graph_, service_name, options); auto cli_base_ptr = std::dynamic_pointer_cast(cli); node_services_->add_client(cli_base_ptr, group); @@ -118,77 +100,68 @@ LifecycleNode::create_client( } template -typename rclcpp::Service::SharedPtr -LifecycleNode::create_service( +typename rclcpp::Service::SharedPtr LifecycleNode::create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, rclcpp::callback_group::CallbackGroup::SharedPtr group) { return rclcpp::create_service( - node_base_, node_services_, - service_name, std::forward(callback), qos_profile, group); + node_base_, + node_services_, + service_name, + std::forward(callback), + qos_profile, + group); } template -auto -LifecycleNode::declare_parameter( +auto LifecycleNode::declare_parameter( const std::string & name, const ParameterT & default_value, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor) { - return this->declare_parameter( - name, - rclcpp::ParameterValue(default_value), - parameter_descriptor - ).get(); + return this->declare_parameter(name, rclcpp::ParameterValue(default_value), parameter_descriptor) + .get(); } template -std::vector -LifecycleNode::declare_parameters( - const std::string & namespace_, - const std::map & parameters) +std::vector LifecycleNode::declare_parameters( + const std::string & namespace_, const std::map & parameters) { std::vector result; std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); std::transform( - parameters.begin(), parameters.end(), std::back_inserter(result), + parameters.begin(), + parameters.end(), + std::back_inserter(result), [this, &normalized_namespace](auto element) { return this->declare_parameter(normalized_namespace + element.first, element.second); - } - ); + }); return result; } template -std::vector -LifecycleNode::declare_parameters( +std::vector LifecycleNode::declare_parameters( const std::string & namespace_, - const std::map< - std::string, - std::pair - > & parameters) + const std::map> & + parameters) { std::vector result; std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); std::transform( - parameters.begin(), parameters.end(), std::back_inserter(result), + parameters.begin(), + parameters.end(), + std::back_inserter(result), [this, &normalized_namespace](auto element) { - return static_cast( - this->declare_parameter( - normalized_namespace + element.first, - element.second.first, - element.second.second) - ); - } - ); + return static_cast(this->declare_parameter( + normalized_namespace + element.first, element.second.first, element.second.second)); + }); return result; } template -bool -LifecycleNode::get_parameter(const std::string & name, ParameterT & parameter) const +bool LifecycleNode::get_parameter(const std::string & name, ParameterT & parameter) const { rclcpp::Parameter param(name, parameter); bool result = get_parameter(name, param); @@ -201,10 +174,8 @@ LifecycleNode::get_parameter(const std::string & name, ParameterT & parameter) c // where our concrete type for ParameterT is std::map, but the to-be-determined // type is the value in the map. template -bool -LifecycleNode::get_parameters( - const std::string & prefix, - std::map & values) const +bool LifecycleNode::get_parameters( + const std::string & prefix, std::map & values) const { std::map params; bool result = node_parameters_->get_parameters_by_prefix(prefix, params); @@ -218,11 +189,8 @@ LifecycleNode::get_parameters( } template -bool -LifecycleNode::get_parameter_or( - const std::string & name, - ParameterT & value, - const ParameterT & alternative_value) const +bool LifecycleNode::get_parameter_or( + const std::string & name, ParameterT & value, const ParameterT & alternative_value) const { bool got_parameter = get_parameter(name, value); if (!got_parameter) { diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index e5d2ada421..8a5eca72a5 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -48,7 +48,7 @@ class LifecyclePublisherInterface */ template> class LifecyclePublisher : public LifecyclePublisherInterface, - public rclcpp::Publisher + public rclcpp::Publisher { public: RCLCPP_SMART_PTR_DEFINITIONS(LifecyclePublisher) @@ -71,7 +71,9 @@ class LifecyclePublisher : public LifecyclePublisherInterface, { } - ~LifecyclePublisher() {} + ~LifecyclePublisher() + { + } /// LifecyclePublisher publish function /** @@ -79,8 +81,7 @@ class LifecyclePublisher : public LifecyclePublisherInterface, * was enabled or disabled and forwards the message * to the actual rclcpp Publisher base class */ - virtual void - publish(std::unique_ptr msg) + virtual void publish(std::unique_ptr msg) { if (!enabled_) { RCLCPP_WARN( @@ -99,8 +100,7 @@ class LifecyclePublisher : public LifecyclePublisherInterface, * was enabled or disabled and forwards the message * to the actual rclcpp Publisher base class */ - virtual void - publish(const MessageT & msg) + virtual void publish(const MessageT & msg) { if (!enabled_) { RCLCPP_WARN( @@ -113,20 +113,17 @@ class LifecyclePublisher : public LifecyclePublisherInterface, rclcpp::Publisher::publish(msg); } - virtual void - on_activate() + virtual void on_activate() { enabled_ = true; } - virtual void - on_deactivate() + virtual void on_deactivate() { enabled_ = false; } - virtual bool - is_activated() + virtual bool is_activated() { return enabled_; } diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index 86ebc22604..152f315685 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -43,11 +43,12 @@ class LifecycleNodeInterface { protected: RCLCPP_LIFECYCLE_PUBLIC - LifecycleNodeInterface() {} + LifecycleNodeInterface() + { + } public: - enum class CallbackReturn : uint8_t - { + enum class CallbackReturn : uint8_t { SUCCESS = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, FAILURE = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE, ERROR = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR @@ -58,48 +59,42 @@ class LifecycleNodeInterface * \return true by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual CallbackReturn - on_configure(const State & previous_state); + virtual CallbackReturn on_configure(const State & previous_state); /// Callback function for cleanup transition /* * \return true by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual CallbackReturn - on_cleanup(const State & previous_state); + virtual CallbackReturn on_cleanup(const State & previous_state); /// Callback function for shutdown transition /* * \return true by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual CallbackReturn - on_shutdown(const State & previous_state); + virtual CallbackReturn on_shutdown(const State & previous_state); /// Callback function for activate transition /* * \return true by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual CallbackReturn - on_activate(const State & previous_state); + virtual CallbackReturn on_activate(const State & previous_state); /// Callback function for deactivate transition /* * \return true by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual CallbackReturn - on_deactivate(const State & previous_state); + virtual CallbackReturn on_deactivate(const State & previous_state); /// Callback function for errorneous transition /* * \return false by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual CallbackReturn - on_error(const State & previous_state); + virtual CallbackReturn on_error(const State & previous_state); }; } // namespace node_interfaces diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index eecbf43fc4..bb03a488b9 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -54,17 +54,14 @@ class State State & operator=(const State & rhs); RCLCPP_LIFECYCLE_PUBLIC - uint8_t - id() const; + uint8_t id() const; RCLCPP_LIFECYCLE_PUBLIC - std::string - label() const; + std::string label() const; protected: RCLCPP_LIFECYCLE_PUBLIC - void - reset(); + void reset(); rcutils_allocator_t allocator_; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp index a566a22264..ee5aca409f 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -42,8 +42,10 @@ class Transition RCLCPP_LIFECYCLE_PUBLIC Transition( - uint8_t id, const std::string & label, - State && start, State && goal, + uint8_t id, + const std::string & label, + State && start, + State && goal, rcutils_allocator_t allocator = rcutils_get_default_allocator()); RCLCPP_LIFECYCLE_PUBLIC @@ -61,25 +63,20 @@ class Transition Transition & operator=(const Transition & rhs); RCLCPP_LIFECYCLE_PUBLIC - uint8_t - id() const; + uint8_t id() const; RCLCPP_LIFECYCLE_PUBLIC - std::string - label() const; + std::string label() const; RCLCPP_LIFECYCLE_PUBLIC - State - start_state() const; + State start_state() const; RCLCPP_LIFECYCLE_PUBLIC - State - goal_state() const; + State goal_state() const; protected: RCLCPP_LIFECYCLE_PUBLIC - void - reset(); + void reset(); rcutils_allocator_t allocator_; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp index 1a5313a191..ea3890b0e8 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp @@ -15,8 +15,8 @@ #ifndef RCLCPP_LIFECYCLE__TYPE_TRAITS__IS_MANAGEABLE_NODE_HPP_ #define RCLCPP_LIFECYCLE__TYPE_TRAITS__IS_MANAGEABLE_NODE_HPP_ -#include #include +#include template struct has_on_activate @@ -50,13 +50,15 @@ struct has_on_deactivate< template struct is_manageable_node : std::false_type -{}; +{ +}; template struct is_manageable_node< T, - typename std::enable_if< - has_on_activate::value && has_on_deactivate::value>::type>: std::true_type -{}; + typename std::enable_if::value && has_on_deactivate::value>::type> +: std::true_type +{ +}; #endif // RCLCPP_LIFECYCLE__TYPE_TRAITS__IS_MANAGEABLE_NODE_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h b/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h index b2a8327a69..5d964be438 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h @@ -26,31 +26,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define RCLCPP_LIFECYCLE_EXPORT __attribute__ ((dllexport)) - #define RCLCPP_LIFECYCLE_IMPORT __attribute__ ((dllimport)) - #else - #define RCLCPP_LIFECYCLE_EXPORT __declspec(dllexport) - #define RCLCPP_LIFECYCLE_IMPORT __declspec(dllimport) - #endif - #ifdef RCLCPP_LIFECYCLE_BUILDING_DLL - #define RCLCPP_LIFECYCLE_PUBLIC RCLCPP_LIFECYCLE_EXPORT - #else - #define RCLCPP_LIFECYCLE_PUBLIC RCLCPP_LIFECYCLE_IMPORT - #endif - #define RCLCPP_LIFECYCLE_PUBLIC_TYPE RCLCPP_LIFECYCLE_PUBLIC - #define RCLCPP_LIFECYCLE_LOCAL +#ifdef __GNUC__ +#define RCLCPP_LIFECYCLE_EXPORT __attribute__((dllexport)) +#define RCLCPP_LIFECYCLE_IMPORT __attribute__((dllimport)) #else - #define RCLCPP_LIFECYCLE_EXPORT __attribute__ ((visibility("default"))) - #define RCLCPP_LIFECYCLE_IMPORT - #if __GNUC__ >= 4 - #define RCLCPP_LIFECYCLE_PUBLIC __attribute__ ((visibility("default"))) - #define RCLCPP_LIFECYCLE_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define RCLCPP_LIFECYCLE_PUBLIC - #define RCLCPP_LIFECYCLE_LOCAL - #endif - #define RCLCPP_LIFECYCLE_PUBLIC_TYPE +#define RCLCPP_LIFECYCLE_EXPORT __declspec(dllexport) +#define RCLCPP_LIFECYCLE_IMPORT __declspec(dllimport) +#endif +#ifdef RCLCPP_LIFECYCLE_BUILDING_DLL +#define RCLCPP_LIFECYCLE_PUBLIC RCLCPP_LIFECYCLE_EXPORT +#else +#define RCLCPP_LIFECYCLE_PUBLIC RCLCPP_LIFECYCLE_IMPORT +#endif +#define RCLCPP_LIFECYCLE_PUBLIC_TYPE RCLCPP_LIFECYCLE_PUBLIC +#define RCLCPP_LIFECYCLE_LOCAL +#else +#define RCLCPP_LIFECYCLE_EXPORT __attribute__((visibility("default"))) +#define RCLCPP_LIFECYCLE_IMPORT +#if __GNUC__ >= 4 +#define RCLCPP_LIFECYCLE_PUBLIC __attribute__((visibility("default"))) +#define RCLCPP_LIFECYCLE_LOCAL __attribute__((visibility("hidden"))) +#else +#define RCLCPP_LIFECYCLE_PUBLIC +#define RCLCPP_LIFECYCLE_LOCAL +#endif +#define RCLCPP_LIFECYCLE_PUBLIC_TYPE #endif #endif // RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 90e7f1cd1d..bf9e9131e7 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -14,11 +14,11 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include #include #include -#include +#include #include +#include #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" @@ -44,60 +44,49 @@ namespace rclcpp_lifecycle { -LifecycleNode::LifecycleNode( - const std::string & node_name, - const rclcpp::NodeOptions & options) -: LifecycleNode( - node_name, - "", - options) -{} +LifecycleNode::LifecycleNode(const std::string & node_name, const rclcpp::NodeOptions & options) +: LifecycleNode(node_name, "", options) +{ +} LifecycleNode::LifecycleNode( const std::string & node_name, const std::string & namespace_, const rclcpp::NodeOptions & options) : node_base_(new rclcpp::node_interfaces::NodeBase( - node_name, - namespace_, - options.context(), - *(options.get_rcl_node_options()), - options.use_intra_process_comms())), + node_name, + namespace_, + options.context(), + *(options.get_rcl_node_options()), + options.use_intra_process_comms())), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), node_clock_(new rclcpp::node_interfaces::NodeClock( - node_base_, - node_topics_, - node_graph_, - node_services_, - node_logging_ - )), + node_base_, node_topics_, node_graph_, node_services_, node_logging_)), node_parameters_(new rclcpp::node_interfaces::NodeParameters( - node_base_, - node_logging_, - node_topics_, - node_services_, - node_clock_, - options.parameter_overrides(), - options.start_parameter_services(), - options.start_parameter_event_publisher(), - options.parameter_event_qos(), - options.parameter_event_publisher_options(), - options.allow_undeclared_parameters(), - options.automatically_declare_parameters_from_overrides() - )), + node_base_, + node_logging_, + node_topics_, + node_services_, + node_clock_, + options.parameter_overrides(), + options.start_parameter_services(), + options.start_parameter_event_publisher(), + options.parameter_event_qos(), + options.parameter_event_publisher_options(), + options.allow_undeclared_parameters(), + options.automatically_declare_parameters_from_overrides())), node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( - node_base_, - node_topics_, - node_graph_, - node_services_, - node_logging_, - node_clock_, - node_parameters_ - )), + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_, + node_clock_, + node_parameters_)), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) @@ -105,55 +94,43 @@ LifecycleNode::LifecycleNode( impl_->init(); register_on_configure( - std::bind( - &LifecycleNodeInterface::on_configure, this, - std::placeholders::_1)); + std::bind(&LifecycleNodeInterface::on_configure, this, std::placeholders::_1)); register_on_cleanup(std::bind(&LifecycleNodeInterface::on_cleanup, this, std::placeholders::_1)); register_on_shutdown( - std::bind( - &LifecycleNodeInterface::on_shutdown, this, - std::placeholders::_1)); + std::bind(&LifecycleNodeInterface::on_shutdown, this, std::placeholders::_1)); register_on_activate( - std::bind( - &LifecycleNodeInterface::on_activate, this, - std::placeholders::_1)); + std::bind(&LifecycleNodeInterface::on_activate, this, std::placeholders::_1)); register_on_deactivate( - std::bind( - &LifecycleNodeInterface::on_deactivate, this, - std::placeholders::_1)); + std::bind(&LifecycleNodeInterface::on_deactivate, this, std::placeholders::_1)); register_on_error(std::bind(&LifecycleNodeInterface::on_error, this, std::placeholders::_1)); } LifecycleNode::~LifecycleNode() -{} +{ +} -const char * -LifecycleNode::get_name() const +const char * LifecycleNode::get_name() const { return node_base_->get_name(); } -const char * -LifecycleNode::get_namespace() const +const char * LifecycleNode::get_namespace() const { return node_base_->get_namespace(); } -rclcpp::Logger -LifecycleNode::get_logger() const +rclcpp::Logger LifecycleNode::get_logger() const { return node_logging_->get_logger(); } -rclcpp::callback_group::CallbackGroup::SharedPtr -LifecycleNode::create_callback_group( +rclcpp::callback_group::CallbackGroup::SharedPtr LifecycleNode::create_callback_group( rclcpp::callback_group::CallbackGroupType group_type) { return node_base_->create_callback_group(group_type); } -const rclcpp::ParameterValue & -LifecycleNode::declare_parameter( +const rclcpp::ParameterValue & LifecycleNode::declare_parameter( const std::string & name, const rclcpp::ParameterValue & default_value, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor) @@ -161,67 +138,57 @@ LifecycleNode::declare_parameter( return this->node_parameters_->declare_parameter(name, default_value, parameter_descriptor); } -void -LifecycleNode::undeclare_parameter(const std::string & name) +void LifecycleNode::undeclare_parameter(const std::string & name) { this->node_parameters_->undeclare_parameter(name); } -bool -LifecycleNode::has_parameter(const std::string & name) const +bool LifecycleNode::has_parameter(const std::string & name) const { return this->node_parameters_->has_parameter(name); } -rcl_interfaces::msg::SetParametersResult -LifecycleNode::set_parameter(const rclcpp::Parameter & parameter) +rcl_interfaces::msg::SetParametersResult LifecycleNode::set_parameter( + const rclcpp::Parameter & parameter) { return this->set_parameters_atomically({parameter}); } -bool -LifecycleNode::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +bool LifecycleNode::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) { return node_base_->callback_group_in_node(group); } -std::vector -LifecycleNode::set_parameters( +std::vector LifecycleNode::set_parameters( const std::vector & parameters) { return node_parameters_->set_parameters(parameters); } -rcl_interfaces::msg::SetParametersResult -LifecycleNode::set_parameters_atomically( +rcl_interfaces::msg::SetParametersResult LifecycleNode::set_parameters_atomically( const std::vector & parameters) { return node_parameters_->set_parameters_atomically(parameters); } -std::vector -LifecycleNode::get_parameters( +std::vector LifecycleNode::get_parameters( const std::vector & names) const { return node_parameters_->get_parameters(names); } -rclcpp::Parameter -LifecycleNode::get_parameter(const std::string & name) const +rclcpp::Parameter LifecycleNode::get_parameter(const std::string & name) const { return node_parameters_->get_parameter(name); } -bool -LifecycleNode::get_parameter( - const std::string & name, - rclcpp::Parameter & parameter) const +bool LifecycleNode::get_parameter(const std::string & name, rclcpp::Parameter & parameter) const { return node_parameters_->get_parameter(name, parameter); } -rcl_interfaces::msg::ParameterDescriptor -LifecycleNode::describe_parameter(const std::string & name) const +rcl_interfaces::msg::ParameterDescriptor LifecycleNode::describe_parameter( + const std::string & name) const { auto result = node_parameters_->describe_parameters({name}); if (0 == result.size()) { @@ -233,59 +200,52 @@ LifecycleNode::describe_parameter(const std::string & name) const return result.front(); } -std::vector -LifecycleNode::describe_parameters( +std::vector LifecycleNode::describe_parameters( const std::vector & names) const { return node_parameters_->describe_parameters(names); } -std::vector -LifecycleNode::get_parameter_types( +std::vector LifecycleNode::get_parameter_types( const std::vector & names) const { return node_parameters_->get_parameter_types(names); } -rcl_interfaces::msg::ListParametersResult -LifecycleNode::list_parameters( +rcl_interfaces::msg::ListParametersResult LifecycleNode::list_parameters( const std::vector & prefixes, uint64_t depth) const { return node_parameters_->list_parameters(prefixes, depth); } -rclcpp::Node::OnParametersSetCallbackType -LifecycleNode::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) +rclcpp::Node::OnParametersSetCallbackType LifecycleNode::set_on_parameters_set_callback( + rclcpp::Node::OnParametersSetCallbackType callback) { return node_parameters_->set_on_parameters_set_callback(callback); } -std::vector -LifecycleNode::get_node_names() const +std::vector LifecycleNode::get_node_names() const { return node_graph_->get_node_names(); } -std::map> -LifecycleNode::get_topic_names_and_types(bool no_demangle) const +std::map> LifecycleNode::get_topic_names_and_types( + bool no_demangle) const { return node_graph_->get_topic_names_and_types(no_demangle); } -std::map> -LifecycleNode::get_service_names_and_types() const +std::map> LifecycleNode::get_service_names_and_types() const { return node_graph_->get_service_names_and_types(); } -size_t -LifecycleNode::count_publishers(const std::string & topic_name) const +size_t LifecycleNode::count_publishers(const std::string & topic_name) const { return node_graph_->count_publishers(topic_name); } -size_t -LifecycleNode::count_subscribers(const std::string & topic_name) const +size_t LifecycleNode::count_subscribers(const std::string & topic_name) const { return node_graph_->count_subscribers(topic_name); } @@ -296,52 +256,43 @@ LifecycleNode::get_callback_groups() const return node_base_->get_callback_groups(); } -rclcpp::Event::SharedPtr -LifecycleNode::get_graph_event() +rclcpp::Event::SharedPtr LifecycleNode::get_graph_event() { return node_graph_->get_graph_event(); } -void -LifecycleNode::wait_for_graph_change( - rclcpp::Event::SharedPtr event, - std::chrono::nanoseconds timeout) +void LifecycleNode::wait_for_graph_change( + rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout) { node_graph_->wait_for_graph_change(event, timeout); } -rclcpp::Clock::SharedPtr -LifecycleNode::get_clock() +rclcpp::Clock::SharedPtr LifecycleNode::get_clock() { return node_clock_->get_clock(); } -rclcpp::Time -LifecycleNode::now() +rclcpp::Time LifecycleNode::now() { return node_clock_->get_clock()->now(); } -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -LifecycleNode::get_node_base_interface() +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr LifecycleNode::get_node_base_interface() { return node_base_; } -rclcpp::node_interfaces::NodeClockInterface::SharedPtr -LifecycleNode::get_node_clock_interface() +rclcpp::node_interfaces::NodeClockInterface::SharedPtr LifecycleNode::get_node_clock_interface() { return node_clock_; } -rclcpp::node_interfaces::NodeGraphInterface::SharedPtr -LifecycleNode::get_node_graph_interface() +rclcpp::node_interfaces::NodeGraphInterface::SharedPtr LifecycleNode::get_node_graph_interface() { return node_graph_; } -rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr -LifecycleNode::get_node_logging_interface() +rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr LifecycleNode::get_node_logging_interface() { return node_logging_; } @@ -352,14 +303,12 @@ LifecycleNode::get_node_time_source_interface() return node_time_source_; } -rclcpp::node_interfaces::NodeTimersInterface::SharedPtr -LifecycleNode::get_node_timers_interface() +rclcpp::node_interfaces::NodeTimersInterface::SharedPtr LifecycleNode::get_node_timers_interface() { return node_timers_; } -rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr -LifecycleNode::get_node_topics_interface() +rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr LifecycleNode::get_node_topics_interface() { return node_topics_; } @@ -382,184 +331,147 @@ LifecycleNode::get_node_waitables_interface() return node_waitables_; } -const rclcpp::NodeOptions & -LifecycleNode::get_node_options() const +const rclcpp::NodeOptions & LifecycleNode::get_node_options() const { return node_options_; } //// -bool -LifecycleNode::register_on_configure( +bool LifecycleNode::register_on_configure( std::function fcn) { - return impl_->register_callback( - lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn); } -bool -LifecycleNode::register_on_cleanup( +bool LifecycleNode::register_on_cleanup( std::function fcn) { - return impl_->register_callback( - lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn); } -bool -LifecycleNode::register_on_shutdown( +bool LifecycleNode::register_on_shutdown( std::function fcn) { - return impl_->register_callback( - lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn); } -bool -LifecycleNode::register_on_activate( +bool LifecycleNode::register_on_activate( std::function fcn) { - return impl_->register_callback( - lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn); } -bool -LifecycleNode::register_on_deactivate( +bool LifecycleNode::register_on_deactivate( std::function fcn) { - return impl_->register_callback( - lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn); + return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn); } -bool -LifecycleNode::register_on_error( +bool LifecycleNode::register_on_error( std::function fcn) { return impl_->register_callback( lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn); } -const State & -LifecycleNode::get_current_state() +const State & LifecycleNode::get_current_state() { return impl_->get_current_state(); } -std::vector -LifecycleNode::get_available_states() +std::vector LifecycleNode::get_available_states() { return impl_->get_available_states(); } -std::vector -LifecycleNode::get_available_transitions() +std::vector LifecycleNode::get_available_transitions() { return impl_->get_available_transitions(); } -const State & -LifecycleNode::trigger_transition(const Transition & transition) +const State & LifecycleNode::trigger_transition(const Transition & transition) { return trigger_transition(transition.id()); } -const State & -LifecycleNode::trigger_transition( +const State & LifecycleNode::trigger_transition( const Transition & transition, LifecycleNodeInterface::CallbackReturn & cb_return_code) { return trigger_transition(transition.id(), cb_return_code); } -const State & -LifecycleNode::trigger_transition(uint8_t transition_id) +const State & LifecycleNode::trigger_transition(uint8_t transition_id) { return impl_->trigger_transition(transition_id); } -const State & -LifecycleNode::trigger_transition( +const State & LifecycleNode::trigger_transition( uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) { return impl_->trigger_transition(transition_id, cb_return_code); } -const State & -LifecycleNode::configure() +const State & LifecycleNode::configure() { - return impl_->trigger_transition( - lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + return impl_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); } -const State & -LifecycleNode::configure(LifecycleNodeInterface::CallbackReturn & cb_return_code) +const State & LifecycleNode::configure(LifecycleNodeInterface::CallbackReturn & cb_return_code) { return impl_->trigger_transition( lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, cb_return_code); } -const State & -LifecycleNode::cleanup() +const State & LifecycleNode::cleanup() { - return impl_->trigger_transition( - lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); + return impl_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); } -const State & -LifecycleNode::cleanup(LifecycleNodeInterface::CallbackReturn & cb_return_code) +const State & LifecycleNode::cleanup(LifecycleNodeInterface::CallbackReturn & cb_return_code) { return impl_->trigger_transition( lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, cb_return_code); } -const State & -LifecycleNode::activate() +const State & LifecycleNode::activate() { - return impl_->trigger_transition( - lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + return impl_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); } -const State & -LifecycleNode::activate(LifecycleNodeInterface::CallbackReturn & cb_return_code) +const State & LifecycleNode::activate(LifecycleNodeInterface::CallbackReturn & cb_return_code) { return impl_->trigger_transition( lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, cb_return_code); } -const State & -LifecycleNode::deactivate() +const State & LifecycleNode::deactivate() { - return impl_->trigger_transition( - lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + return impl_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); } -const State & -LifecycleNode::deactivate(LifecycleNodeInterface::CallbackReturn & cb_return_code) +const State & LifecycleNode::deactivate(LifecycleNodeInterface::CallbackReturn & cb_return_code) { return impl_->trigger_transition( lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, cb_return_code); } -const State & -LifecycleNode::shutdown() +const State & LifecycleNode::shutdown() { - return impl_->trigger_transition( - rcl_lifecycle_shutdown_label); + return impl_->trigger_transition(rcl_lifecycle_shutdown_label); } -const State & -LifecycleNode::shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code) +const State & LifecycleNode::shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code) { - return impl_->trigger_transition( - rcl_lifecycle_shutdown_label, cb_return_code); + return impl_->trigger_transition(rcl_lifecycle_shutdown_label, cb_return_code); } -void -LifecycleNode::add_publisher_handle( +void LifecycleNode::add_publisher_handle( std::shared_ptr pub) { impl_->add_publisher_handle(pub); } -void -LifecycleNode::add_timer_handle(std::shared_ptr timer) +void LifecycleNode::add_timer_handle(std::shared_ptr timer) { impl_->add_timer_handle(timer); } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 5dc81e9629..057eeaba99 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -21,16 +21,16 @@ #include #include #include -#include #include +#include #include "lifecycle_msgs/msg/transition_description.hpp" #include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport #include "lifecycle_msgs/msg/transition_event.hpp" #include "lifecycle_msgs/srv/change_state.hpp" -#include "lifecycle_msgs/srv/get_state.hpp" #include "lifecycle_msgs/srv/get_available_states.hpp" #include "lifecycle_msgs/srv/get_available_transitions.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" #include "rcl/error_handling.h" @@ -57,25 +57,22 @@ class LifecycleNode::LifecycleNodeInterfaceImpl LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, std::shared_ptr node_services_interface) - : node_base_interface_(node_base_interface), - node_services_interface_(node_services_interface) - {} + : node_base_interface_(node_base_interface), node_services_interface_(node_services_interface) + { + } ~LifecycleNodeInterfaceImpl() { rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); const rcl_node_options_t * node_options = rcl_node_get_options(node_handle); - auto ret = rcl_lifecycle_state_machine_fini( - &state_machine_, node_handle, &node_options->allocator); + auto ret = + rcl_lifecycle_state_machine_fini(&state_machine_, node_handle, &node_options->allocator); if (ret != RCL_RET_OK) { - RCUTILS_LOG_FATAL_NAMED( - "rclcpp_lifecycle", - "failed to destroy rcl_state_machine"); + RCUTILS_LOG_FATAL_NAMED("rclcpp_lifecycle", "failed to destroy rcl_state_machine"); } } - void - init() + void init() { rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); const rcl_node_options_t * node_options = @@ -100,14 +97,17 @@ class LifecycleNode::LifecycleNodeInterfaceImpl &node_options->allocator); if (ret != RCL_RET_OK) { throw std::runtime_error( - std::string("Couldn't initialize state machine for node ") + - node_base_interface_->get_name()); + std::string("Couldn't initialize state machine for node ") + + node_base_interface_->get_name()); } { // change_state auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_change_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + &LifecycleNodeInterfaceImpl::on_change_state, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3); rclcpp::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); @@ -116,14 +116,16 @@ class LifecycleNode::LifecycleNodeInterfaceImpl &state_machine_.com_interface.srv_change_state, any_cb); node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_change_state_), - nullptr); + std::dynamic_pointer_cast(srv_change_state_), nullptr); } { // get_state auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + &LifecycleNodeInterfaceImpl::on_get_state, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3); rclcpp::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); @@ -132,14 +134,16 @@ class LifecycleNode::LifecycleNodeInterfaceImpl &state_machine_.com_interface.srv_get_state, any_cb); node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_state_), - nullptr); + std::dynamic_pointer_cast(srv_get_state_), nullptr); } { // get_available_states auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_available_states, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + &LifecycleNodeInterfaceImpl::on_get_available_states, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3); rclcpp::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); @@ -148,47 +152,48 @@ class LifecycleNode::LifecycleNodeInterfaceImpl &state_machine_.com_interface.srv_get_available_states, any_cb); node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_states_), - nullptr); + std::dynamic_pointer_cast(srv_get_available_states_), nullptr); } { // get_available_transitions auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_available_transitions, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + &LifecycleNodeInterfaceImpl::on_get_available_transitions, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3); rclcpp::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); srv_get_available_transitions_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_available_transitions, - any_cb); + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_transitions, + any_cb); node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_transitions_), - nullptr); + std::dynamic_pointer_cast(srv_get_available_transitions_), nullptr); } { // get_transition_graph auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_transition_graph, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + &LifecycleNodeInterfaceImpl::on_get_transition_graph, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3); rclcpp::AnyServiceCallback any_cb; any_cb.set(std::move(cb)); - srv_get_transition_graph_ = - std::make_shared>( + srv_get_transition_graph_ = std::make_shared>( node_base_interface_->get_shared_rcl_node_handle(), &state_machine_.com_interface.srv_get_transition_graph, any_cb); node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_transition_graph_), - nullptr); + std::dynamic_pointer_cast(srv_get_transition_graph_), nullptr); } } - bool - register_callback( + bool register_callback( std::uint8_t lifecycle_transition, std::function & cb) { @@ -196,16 +201,14 @@ class LifecycleNode::LifecycleNodeInterfaceImpl return true; } - void - on_change_state( + void on_change_state( const std::shared_ptr header, const std::shared_ptr req, std::shared_ptr resp) { (void)header; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get state. State machine is not initialized."); + throw std::runtime_error("Can't get state. State machine is not initialized."); } auto transition_id = req->transition.id; @@ -229,7 +232,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code; auto ret = change_state(transition_id, cb_return_code); - (void) ret; + (void)ret; // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns // 1. return is the actual transition // 2. return is whether an error occurred or not @@ -237,8 +240,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); } - void - on_get_state( + void on_get_state( const std::shared_ptr header, const std::shared_ptr req, std::shared_ptr resp) @@ -246,15 +248,13 @@ class LifecycleNode::LifecycleNodeInterfaceImpl (void)header; (void)req; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get state. State machine is not initialized."); + throw std::runtime_error("Can't get state. State machine is not initialized."); } resp->current_state.id = static_cast(state_machine_.current_state->id); resp->current_state.label = state_machine_.current_state->label; } - void - on_get_available_states( + void on_get_available_states( const std::shared_ptr header, const std::shared_ptr req, std::shared_ptr resp) @@ -262,8 +262,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl (void)header; (void)req; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available states. State machine is not initialized."); + throw std::runtime_error("Can't get available states. State machine is not initialized."); } for (uint8_t i = 0; i < state_machine_.transition_map.states_size; ++i) { lifecycle_msgs::msg::State state; @@ -273,8 +272,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } } - void - on_get_available_transitions( + void on_get_available_transitions( const std::shared_ptr header, const std::shared_ptr req, std::shared_ptr resp) @@ -283,7 +281,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl (void)req; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { throw std::runtime_error( - "Can't get available transitions. State machine is not initialized."); + "Can't get available transitions. State machine is not initialized."); } for (uint8_t i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { @@ -299,8 +297,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } } - void - on_get_transition_graph( + void on_get_transition_graph( const std::shared_ptr header, const std::shared_ptr req, std::shared_ptr resp) @@ -309,7 +306,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl (void)req; if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { throw std::runtime_error( - "Can't get available transitions. State machine is not initialized."); + "Can't get available transitions. State machine is not initialized."); } for (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) { @@ -325,15 +322,13 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } } - const State & - get_current_state() + const State & get_current_state() { current_state_ = State(state_machine_.current_state); return current_state_; } - std::vector - get_available_states() + std::vector get_available_states() { std::vector states; for (uint8_t i = 0; i < state_machine_.transition_map.states_size; ++i) { @@ -343,26 +338,25 @@ class LifecycleNode::LifecycleNodeInterfaceImpl return states; } - std::vector - get_available_transitions() + std::vector get_available_transitions() { std::vector transitions; for (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) { - Transition transition( - &state_machine_.transition_map.transitions[i]); + Transition transition(&state_machine_.transition_map.transitions[i]); transitions.push_back(transition); } return transitions; } - rcl_ret_t - change_state(std::uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) + rcl_ret_t change_state( + std::uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) { if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { RCUTILS_LOG_ERROR( "Unable to change state for state machine for %s: %s", - node_base_interface_->get_name(), rcl_get_error_string().str); + node_base_interface_->get_name(), + rcl_get_error_string().str); return RCL_RET_ERROR; } @@ -371,37 +365,39 @@ class LifecycleNode::LifecycleNodeInterfaceImpl State initial_state(state_machine_.current_state); if ( - rcl_lifecycle_trigger_transition_by_id( - &state_machine_, transition_id, publish_update) != RCL_RET_OK) - { + rcl_lifecycle_trigger_transition_by_id(&state_machine_, transition_id, publish_update) != + RCL_RET_OK) { RCUTILS_LOG_ERROR( "Unable to start transition %u from current state %s: %s", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + transition_id, + state_machine_.current_state->label, + rcl_get_error_string().str); rcutils_reset_error(); return RCL_RET_ERROR; } auto get_label_for_return_code = - [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{ - auto cb_id = static_cast(cb_return_code); - if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { - return rcl_lifecycle_transition_success_label; - } else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) { - return rcl_lifecycle_transition_failure_label; - } - return rcl_lifecycle_transition_error_label; - }; + [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char * { + auto cb_id = static_cast(cb_return_code); + if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { + return rcl_lifecycle_transition_success_label; + } else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) { + return rcl_lifecycle_transition_failure_label; + } + return rcl_lifecycle_transition_error_label; + }; cb_return_code = execute_callback(state_machine_.current_state->id, initial_state); auto transition_label = get_label_for_return_code(cb_return_code); if ( rcl_lifecycle_trigger_transition_by_label( - &state_machine_, transition_label, publish_update) != RCL_RET_OK) - { + &state_machine_, transition_label, publish_update) != RCL_RET_OK) { RCUTILS_LOG_ERROR( "Failed to finish transition %u. Current state is now: %s (%s)", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + transition_id, + state_machine_.current_state->label, + rcl_get_error_string().str); rcutils_reset_error(); return RCL_RET_ERROR; } @@ -415,8 +411,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl auto error_cb_label = get_label_for_return_code(error_cb_code); if ( rcl_lifecycle_trigger_transition_by_label( - &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) - { + &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) { RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); rcutils_reset_error(); return RCL_RET_ERROR; @@ -428,8 +423,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl return RCL_RET_OK; } - LifecycleNodeInterface::CallbackReturn - execute_callback(unsigned int cb_id, const State & previous_state) + LifecycleNodeInterface::CallbackReturn execute_callback( + unsigned int cb_id, const State & previous_state) { // in case no callback was attached, we forward directly auto cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -465,50 +460,44 @@ class LifecycleNode::LifecycleNodeInterfaceImpl return get_current_state(); } - const State & - trigger_transition(uint8_t transition_id) + const State & trigger_transition(uint8_t transition_id) { LifecycleNodeInterface::CallbackReturn error; change_state(transition_id, error); - (void) error; + (void)error; return get_current_state(); } - const State & - trigger_transition(uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) + const State & trigger_transition( + uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) { change_state(transition_id, cb_return_code); return get_current_state(); } - void - add_publisher_handle(std::shared_ptr pub) + void add_publisher_handle(std::shared_ptr pub) { weak_pubs_.push_back(pub); } - void - add_timer_handle(std::shared_ptr timer) + void add_timer_handle(std::shared_ptr timer) { weak_timers_.push_back(timer); } rcl_lifecycle_state_machine_t state_machine_; State current_state_; - std::map< - std::uint8_t, - std::function> cb_map_; + std::map> + cb_map_; using NodeBasePtr = std::shared_ptr; using NodeServicesPtr = std::shared_ptr; using ChangeStateSrvPtr = std::shared_ptr>; using GetStateSrvPtr = std::shared_ptr>; - using GetAvailableStatesSrvPtr = - std::shared_ptr>; + using GetAvailableStatesSrvPtr = std::shared_ptr>; using GetAvailableTransitionsSrvPtr = std::shared_ptr>; - using GetTransitionGraphSrvPtr = - std::shared_ptr>; + using GetTransitionGraphSrvPtr = std::shared_ptr>; NodeBasePtr node_base_interface_; NodeServicesPtr node_services_interface_; diff --git a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp index 1c8a5a646a..c403127f5f 100644 --- a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp +++ b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp @@ -21,38 +21,32 @@ namespace rclcpp_lifecycle namespace node_interfaces { -LifecycleNodeInterface::CallbackReturn -LifecycleNodeInterface::on_configure(const State &) +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_configure(const State &) { return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -LifecycleNodeInterface::CallbackReturn -LifecycleNodeInterface::on_cleanup(const State &) +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_cleanup(const State &) { return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -LifecycleNodeInterface::CallbackReturn -LifecycleNodeInterface::on_shutdown(const State &) +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_shutdown(const State &) { return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -LifecycleNodeInterface::CallbackReturn -LifecycleNodeInterface::on_activate(const State &) +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_activate(const State &) { return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -LifecycleNodeInterface::CallbackReturn -LifecycleNodeInterface::on_deactivate(const State &) +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_deactivate(const State &) { return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -LifecycleNodeInterface::CallbackReturn -LifecycleNodeInterface::on_error(const State &) +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_error(const State &) { return LifecycleNodeInterface::CallbackReturn::SUCCESS; } diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index a84f1d301f..7e8ae5d3f3 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/rclcpp_lifecycle/src/state.cpp @@ -29,15 +29,11 @@ namespace rclcpp_lifecycle State::State(rcutils_allocator_t allocator) : State(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, "unknown", allocator) -{} +{ +} -State::State( - uint8_t id, - const std::string & label, - rcutils_allocator_t allocator) -: allocator_(allocator), - owns_rcl_state_handle_(true), - state_handle_(nullptr) +State::State(uint8_t id, const std::string & label, rcutils_allocator_t allocator) +: allocator_(allocator), owns_rcl_state_handle_(true), state_handle_(nullptr) { if (label.empty()) { throw std::runtime_error("Lifecycle State cannot have an empty label."); @@ -60,11 +56,8 @@ State::State( } State::State( - const rcl_lifecycle_state_t * rcl_lifecycle_state_handle, - rcutils_allocator_t allocator) -: allocator_(allocator), - owns_rcl_state_handle_(false), - state_handle_(nullptr) + const rcl_lifecycle_state_t * rcl_lifecycle_state_handle, rcutils_allocator_t allocator) +: allocator_(allocator), owns_rcl_state_handle_(false), state_handle_(nullptr) { if (!rcl_lifecycle_state_handle) { throw std::runtime_error("rcl_lifecycle_state_handle is null"); @@ -73,9 +66,7 @@ State::State( } State::State(const State & rhs) -: allocator_(rhs.allocator_), - owns_rcl_state_handle_(false), - state_handle_(nullptr) +: allocator_(rhs.allocator_), owns_rcl_state_handle_(false), state_handle_(nullptr) { *this = rhs; } @@ -85,8 +76,7 @@ State::~State() reset(); } -State & -State::operator=(const State & rhs) +State & State::operator=(const State & rhs) { if (this == &rhs) { return *this; @@ -114,8 +104,7 @@ State::operator=(const State & rhs) state_handle_->id = 0; state_handle_->label = nullptr; - auto ret = rcl_lifecycle_state_init( - state_handle_, rhs.id(), rhs.label().c_str(), &allocator_); + auto ret = rcl_lifecycle_state_init(state_handle_, rhs.id(), rhs.label().c_str(), &allocator_); if (ret != RCL_RET_OK) { reset(); throw std::runtime_error("failed to duplicate label for rcl_lifecycle_state_t"); @@ -124,8 +113,7 @@ State::operator=(const State & rhs) return *this; } -uint8_t -State::id() const +uint8_t State::id() const { if (!state_handle_) { throw std::runtime_error("Error in state! Internal state_handle is NULL."); @@ -133,8 +121,7 @@ State::id() const return state_handle_->id; } -std::string -State::label() const +std::string State::label() const { if (!state_handle_) { throw std::runtime_error("Error in state! Internal state_handle is NULL."); @@ -142,8 +129,7 @@ State::label() const return state_handle_->label; } -void -State::reset() +void State::reset() { if (!owns_rcl_state_handle_) { state_handle_ = nullptr; diff --git a/rclcpp_lifecycle/src/transition.cpp b/rclcpp_lifecycle/src/transition.cpp index d7bae6c280..ce3297cc09 100644 --- a/rclcpp_lifecycle/src/transition.cpp +++ b/rclcpp_lifecycle/src/transition.cpp @@ -27,13 +27,8 @@ namespace rclcpp_lifecycle { -Transition::Transition( - uint8_t id, - const std::string & label, - rcutils_allocator_t allocator) -: allocator_(allocator), - owns_rcl_transition_handle_(true), - transition_handle_(nullptr) +Transition::Transition(uint8_t id, const std::string & label, rcutils_allocator_t allocator) +: allocator_(allocator), owns_rcl_transition_handle_(true), transition_handle_(nullptr) { transition_handle_ = static_cast( allocator_.allocate(sizeof(rcl_lifecycle_transition_t), allocator_.state)); @@ -55,8 +50,10 @@ Transition::Transition( } Transition::Transition( - uint8_t id, const std::string & label, - State && start, State && goal, + uint8_t id, + const std::string & label, + State && start, + State && goal, rcutils_allocator_t allocator) : Transition(id, label, allocator) { @@ -96,11 +93,8 @@ Transition::Transition( } Transition::Transition( - const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle, - rcutils_allocator_t allocator) -: allocator_(allocator), - owns_rcl_transition_handle_(false), - transition_handle_(nullptr) + const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle, rcutils_allocator_t allocator) +: allocator_(allocator), owns_rcl_transition_handle_(false), transition_handle_(nullptr) { if (!rcl_lifecycle_transition_handle) { throw std::runtime_error("rcl_lifecycle_transition_handle is null"); @@ -109,9 +103,7 @@ Transition::Transition( } Transition::Transition(const Transition & rhs) -: allocator_(rhs.allocator_), - owns_rcl_transition_handle_(false), - transition_handle_(nullptr) +: allocator_(rhs.allocator_), owns_rcl_transition_handle_(false), transition_handle_(nullptr) { *this = rhs; } @@ -121,8 +113,7 @@ Transition::~Transition() reset(); } -Transition & -Transition::operator=(const Transition & rhs) +Transition & Transition::operator=(const Transition & rhs) { if (this == &rhs) { return *this; @@ -207,8 +198,7 @@ Transition::operator=(const Transition & rhs) return *this; } -uint8_t -Transition::id() const +uint8_t Transition::id() const { if (!transition_handle_) { throw std::runtime_error("internal transition_handle is null"); @@ -216,8 +206,7 @@ Transition::id() const return transition_handle_->id; } -std::string -Transition::label() const +std::string Transition::label() const { if (!transition_handle_) { throw std::runtime_error("internal transition_handle is null"); @@ -225,8 +214,7 @@ Transition::label() const return transition_handle_->label; } -State -Transition::start_state() const +State Transition::start_state() const { if (!transition_handle_) { throw std::runtime_error("internal transition_handle is null"); @@ -235,8 +223,7 @@ Transition::start_state() const return State(transition_handle_->start, allocator_); } -State -Transition::goal_state() const +State Transition::goal_state() const { if (!transition_handle_) { throw std::runtime_error("internal transition_handle is null"); @@ -245,8 +232,7 @@ Transition::goal_state() const return State(transition_handle_->goal, allocator_); } -void -Transition::reset() +void Transition::reset() { // can't free anything which is not owned if (!owns_rcl_transition_handle_) { diff --git a/rclcpp_lifecycle/test/test_callback_exceptions.cpp b/rclcpp_lifecycle/test/test_callback_exceptions.cpp index 7c4117ae7b..8ddac7f4b7 100644 --- a/rclcpp_lifecycle/test/test_callback_exceptions.cpp +++ b/rclcpp_lifecycle/test/test_callback_exceptions.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include #include #include @@ -41,38 +40,42 @@ class PositiveCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode public: explicit PositiveCallbackExceptionNode(std::string node_name) : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) - {} + { + } size_t number_of_callbacks = 0; protected: - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &) { ++number_of_callbacks; throw std::runtime_error("custom exception raised in configure callback"); } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_error(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_error( + const rclcpp_lifecycle::State &) { ++number_of_callbacks; return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } }; -TEST_F(TestCallbackExceptions, positive_on_error) { +TEST_F(TestCallbackExceptions, positive_on_error) +{ auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ( - State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + State::PRIMARY_STATE_UNCONFIGURED, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)) + .id()); // check if all callbacks were successfully overwritten EXPECT_EQ(2u, test_node->number_of_callbacks); } -TEST_F(TestCallbackExceptions, positive_on_error_with_code) { +TEST_F(TestCallbackExceptions, positive_on_error_with_code) +{ auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); @@ -87,38 +90,42 @@ class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode public: explicit NegativeCallbackExceptionNode(std::string node_name) : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) - {} + { + } size_t number_of_callbacks = 0; protected: - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &) { ++number_of_callbacks; throw std::runtime_error("custom exception raised in configure callback"); } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_error(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_error( + const rclcpp_lifecycle::State &) { ++number_of_callbacks; return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } }; -TEST_F(TestCallbackExceptions, negative_on_error) { +TEST_F(TestCallbackExceptions, negative_on_error) +{ auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ( - State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + State::PRIMARY_STATE_FINALIZED, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)) + .id()); // check if all callbacks were successfully overwritten EXPECT_EQ(2u, test_node->number_of_callbacks); } -TEST_F(TestCallbackExceptions, negative_on_error_with_code) { +TEST_F(TestCallbackExceptions, negative_on_error_with_code) +{ auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index e8066370e0..9535bbf342 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include #include #include @@ -41,22 +40,21 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode public: explicit EmptyLifecycleNode(std::string node_name) : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) - {} + { + } }; struct GoodMood { - using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - static constexpr CallbackReturnT cb_ret = static_cast( - lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS); + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + static constexpr CallbackReturnT cb_ret = + static_cast(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS); }; struct BadMood { - using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - static constexpr CallbackReturnT cb_ret = static_cast( - lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE); + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + static constexpr CallbackReturnT cb_ret = + static_cast(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE); }; template @@ -65,53 +63,54 @@ class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode public: explicit MoodyLifecycleNode(std::string node_name) : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) - {} + { + } size_t number_of_callbacks = 0; protected: - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); ++number_of_callbacks; return Mood::cb_ret; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); ++number_of_callbacks; return Mood::cb_ret; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); ++number_of_callbacks; return Mood::cb_ret; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); ++number_of_callbacks; return Mood::cb_ret; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); ++number_of_callbacks; return Mood::cb_ret; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_error(const rclcpp_lifecycle::State &); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_error( + const rclcpp_lifecycle::State &); }; template<> @@ -132,35 +131,45 @@ MoodyLifecycleNode::on_error(const rclcpp_lifecycle::State &) return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -TEST_F(TestDefaultStateMachine, empty_initializer) { +TEST_F(TestDefaultStateMachine, empty_initializer) +{ auto test_node = std::make_shared("testnode"); EXPECT_STREQ("testnode", test_node->get_name()); EXPECT_STREQ("/", test_node->get_namespace()); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); } -TEST_F(TestDefaultStateMachine, trigger_transition) { +TEST_F(TestDefaultStateMachine, trigger_transition) +{ auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); ASSERT_EQ( - State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + State::PRIMARY_STATE_INACTIVE, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)) + .id()); ASSERT_EQ( - State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + State::PRIMARY_STATE_ACTIVE, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)) + .id()); ASSERT_EQ( - State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + State::PRIMARY_STATE_INACTIVE, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)) + .id()); ASSERT_EQ( - State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + State::PRIMARY_STATE_UNCONFIGURED, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)) + .id()); ASSERT_EQ( - State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); + State::PRIMARY_STATE_FINALIZED, + test_node + ->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)) + .id()); } -TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) { +TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) +{ auto test_node = std::make_shared("testnode"); auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -188,46 +197,57 @@ TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) { EXPECT_EQ(success, ret); } -TEST_F(TestDefaultStateMachine, good_mood) { +TEST_F(TestDefaultStateMachine, good_mood) +{ auto test_node = std::make_shared>("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ( - State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + State::PRIMARY_STATE_INACTIVE, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)) + .id()); EXPECT_EQ( - State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + State::PRIMARY_STATE_ACTIVE, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)) + .id()); EXPECT_EQ( - State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + State::PRIMARY_STATE_INACTIVE, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)) + .id()); EXPECT_EQ( - State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + State::PRIMARY_STATE_UNCONFIGURED, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)) + .id()); EXPECT_EQ( - State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); + State::PRIMARY_STATE_FINALIZED, + test_node + ->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)) + .id()); // check if all callbacks were successfully overwritten EXPECT_EQ(5u, test_node->number_of_callbacks); } -TEST_F(TestDefaultStateMachine, bad_mood) { +TEST_F(TestDefaultStateMachine, bad_mood) +{ auto test_node = std::make_shared>("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ( - State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + State::PRIMARY_STATE_UNCONFIGURED, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)) + .id()); // check if all callbacks were successfully overwritten EXPECT_EQ(1u, test_node->number_of_callbacks); } -TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { +TEST_F(TestDefaultStateMachine, lifecycle_subscriber) +{ auto test_node = std::make_shared>("testnode"); - auto cb = [](const std::shared_ptr msg) {(void) msg;}; + auto cb = [](const std::shared_ptr msg) { (void)msg; }; auto lifecycle_sub = test_node->create_subscription("~/empty", 10, cb); diff --git a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp index 3c146e4207..cc898c8edf 100644 --- a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp +++ b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include #include #include @@ -41,45 +40,46 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode public: explicit CustomLifecycleNode(std::string node_name) : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) - {} + { + } size_t number_of_callbacks = 0; protected: - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; @@ -89,8 +89,8 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode // Custom callbacks public: - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_custom_configure(const rclcpp_lifecycle::State & previous_state) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_custom_configure( + const rclcpp_lifecycle::State & previous_state) { EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); @@ -98,8 +98,8 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_custom_activate(const rclcpp_lifecycle::State & previous_state) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_custom_activate( + const rclcpp_lifecycle::State & previous_state) { EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); @@ -107,8 +107,8 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_custom_deactivate(const rclcpp_lifecycle::State & previous_state) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_custom_deactivate( + const rclcpp_lifecycle::State & previous_state) { EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); @@ -116,8 +116,8 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_custom_cleanup(const rclcpp_lifecycle::State & previous_state) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_custom_cleanup( + const rclcpp_lifecycle::State & previous_state) { EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); @@ -125,8 +125,8 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_custom_shutdown(const rclcpp_lifecycle::State &) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_custom_shutdown( + const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); ++number_of_callbacks; @@ -134,46 +134,44 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode } }; -TEST_F(TestRegisterCustomCallbacks, custom_callbacks) { +TEST_F(TestRegisterCustomCallbacks, custom_callbacks) +{ auto test_node = std::make_shared("testnode"); test_node->register_on_configure( - std::bind( - &CustomLifecycleNode::on_custom_configure, - test_node.get(), std::placeholders::_1)); + std::bind(&CustomLifecycleNode::on_custom_configure, test_node.get(), std::placeholders::_1)); test_node->register_on_cleanup( - std::bind( - &CustomLifecycleNode::on_custom_cleanup, - test_node.get(), std::placeholders::_1)); + std::bind(&CustomLifecycleNode::on_custom_cleanup, test_node.get(), std::placeholders::_1)); test_node->register_on_shutdown( - std::bind( - &CustomLifecycleNode::on_custom_shutdown, - test_node.get(), std::placeholders::_1)); + std::bind(&CustomLifecycleNode::on_custom_shutdown, test_node.get(), std::placeholders::_1)); test_node->register_on_activate( - std::bind( - &CustomLifecycleNode::on_custom_activate, - test_node.get(), std::placeholders::_1)); + std::bind(&CustomLifecycleNode::on_custom_activate, test_node.get(), std::placeholders::_1)); test_node->register_on_deactivate( - std::bind( - &CustomLifecycleNode::on_custom_deactivate, - test_node.get(), std::placeholders::_1)); + std::bind(&CustomLifecycleNode::on_custom_deactivate, test_node.get(), std::placeholders::_1)); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); EXPECT_EQ( - State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + State::PRIMARY_STATE_INACTIVE, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)) + .id()); EXPECT_EQ( - State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + State::PRIMARY_STATE_ACTIVE, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)) + .id()); EXPECT_EQ( - State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + State::PRIMARY_STATE_INACTIVE, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)) + .id()); EXPECT_EQ( - State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + State::PRIMARY_STATE_UNCONFIGURED, + test_node->trigger_transition(rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)) + .id()); EXPECT_EQ( - State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); + State::PRIMARY_STATE_FINALIZED, + test_node + ->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)) + .id()); // check if all callbacks were successfully overwritten EXPECT_EQ(5u, test_node->number_of_callbacks); diff --git a/rclcpp_lifecycle/test/test_state_machine_info.cpp b/rclcpp_lifecycle/test/test_state_machine_info.cpp index dfa7bdf1ca..0f325e7084 100644 --- a/rclcpp_lifecycle/test/test_state_machine_info.cpp +++ b/rclcpp_lifecycle/test/test_state_machine_info.cpp @@ -30,10 +30,10 @@ class TestStateMachineInfo : public ::testing::Test } }; -TEST_F(TestStateMachineInfo, available_states) { +TEST_F(TestStateMachineInfo, available_states) +{ auto test_node = std::make_shared("testnode"); - std::vector available_states = - test_node->get_available_states(); + std::vector available_states = test_node->get_available_states(); EXPECT_EQ(11u, available_states.size()); // Primary States @@ -44,15 +44,16 @@ TEST_F(TestStateMachineInfo, available_states) { EXPECT_EQ(4, available_states[4].id()); // finalized // Transition States - EXPECT_EQ(10, available_states[5].id()); // configuring - EXPECT_EQ(11, available_states[6].id()); // cleaningup - EXPECT_EQ(12, available_states[7].id()); // shuttingdown - EXPECT_EQ(13, available_states[8].id()); // activating - EXPECT_EQ(14, available_states[9].id()); // deactivating + EXPECT_EQ(10, available_states[5].id()); // configuring + EXPECT_EQ(11, available_states[6].id()); // cleaningup + EXPECT_EQ(12, available_states[7].id()); // shuttingdown + EXPECT_EQ(13, available_states[8].id()); // activating + EXPECT_EQ(14, available_states[9].id()); // deactivating EXPECT_EQ(15, available_states[10].id()); // errorprocessing } -TEST_F(TestStateMachineInfo, available_transitions) { +TEST_F(TestStateMachineInfo, available_transitions) +{ auto test_node = std::make_shared("testnode"); std::vector available_transitions = test_node->get_available_transitions(); @@ -62,14 +63,12 @@ TEST_F(TestStateMachineInfo, available_transitions) { EXPECT_TRUE( transition.start_state().id() <= 4 || - (transition.start_state().id() >= 10 && - (transition.start_state().id() <= 15))); + (transition.start_state().id() >= 10 && (transition.start_state().id() <= 15))); EXPECT_FALSE(transition.start_state().label().empty()); EXPECT_TRUE( transition.goal_state().id() <= 4 || - (transition.goal_state().id() >= 10 && - (transition.goal_state().id() <= 15))); + (transition.goal_state().id() >= 10 && (transition.goal_state().id() <= 15))); EXPECT_FALSE(transition.goal_state().label().empty()); } } diff --git a/rclcpp_lifecycle/test/test_state_wrapper.cpp b/rclcpp_lifecycle/test/test_state_wrapper.cpp index 5d20759367..b5a997d48e 100644 --- a/rclcpp_lifecycle/test/test_state_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_state_wrapper.cpp @@ -28,7 +28,8 @@ class TestStateWrapper : public ::testing::Test } }; -TEST_F(TestStateWrapper, wrapper) { +TEST_F(TestStateWrapper, wrapper) +{ { rclcpp_lifecycle::State state(1, "my_state"); EXPECT_EQ(1, state.id()); @@ -60,8 +61,7 @@ TEST_F(TestStateWrapper, wrapper) { } { - rcl_lifecycle_state_t * lc_state = - new rcl_lifecycle_state_t {"my_c_state", 3, NULL, 0}; + rcl_lifecycle_state_t * lc_state = new rcl_lifecycle_state_t{"my_c_state", 3, NULL, 0}; rclcpp_lifecycle::State c_state(lc_state->id, lc_state->label); EXPECT_EQ(3, c_state.id()); EXPECT_FALSE(c_state.label().empty()); @@ -83,7 +83,8 @@ TEST_F(TestStateWrapper, wrapper) { // } } -TEST_F(TestStateWrapper, copy_constructor) { +TEST_F(TestStateWrapper, copy_constructor) +{ auto a = std::make_shared(1, "my_c_state"); rclcpp_lifecycle::State b(*a); @@ -93,7 +94,8 @@ TEST_F(TestStateWrapper, copy_constructor) { EXPECT_STREQ("my_c_state", b.label().c_str()); } -TEST_F(TestStateWrapper, assignment_operator) { +TEST_F(TestStateWrapper, assignment_operator) +{ auto a = std::make_shared(1, "one"); auto b = std::make_shared(2, "two"); *b = *a; @@ -104,15 +106,14 @@ TEST_F(TestStateWrapper, assignment_operator) { EXPECT_STREQ("one", b->label().c_str()); } -TEST_F(TestStateWrapper, assignment_operator2) { +TEST_F(TestStateWrapper, assignment_operator2) +{ // Non-owning State - rcl_lifecycle_state_t * lc_state1 = - new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; + rcl_lifecycle_state_t * lc_state1 = new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; auto non_owning_state1 = std::make_shared(lc_state1); // Non-owning State - rcl_lifecycle_state_t * lc_state2 = - new rcl_lifecycle_state_t{"my_c_state2", 2, NULL, 0}; + rcl_lifecycle_state_t * lc_state2 = new rcl_lifecycle_state_t{"my_c_state2", 2, NULL, 0}; auto non_owning_state2 = std::make_shared(lc_state2); *non_owning_state2 = *non_owning_state1; @@ -127,10 +128,10 @@ TEST_F(TestStateWrapper, assignment_operator2) { delete lc_state2; } -TEST_F(TestStateWrapper, assignment_operator3) { +TEST_F(TestStateWrapper, assignment_operator3) +{ // Non-owning State - rcl_lifecycle_state_t * lc_state1 = - new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; + rcl_lifecycle_state_t * lc_state1 = new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; auto non_owning_state1 = std::make_shared(lc_state1); // owning State @@ -147,10 +148,10 @@ TEST_F(TestStateWrapper, assignment_operator3) { delete lc_state1; } -TEST_F(TestStateWrapper, assignment_operator4) { +TEST_F(TestStateWrapper, assignment_operator4) +{ // Non-owning State - rcl_lifecycle_state_t * lc_state1 = - new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; + rcl_lifecycle_state_t * lc_state1 = new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; auto non_owning_state1 = std::make_shared(lc_state1); // owning State diff --git a/rclcpp_lifecycle/test/test_transition_wrapper.cpp b/rclcpp_lifecycle/test/test_transition_wrapper.cpp index b920c74135..5b58a62421 100644 --- a/rclcpp_lifecycle/test/test_transition_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_transition_wrapper.cpp @@ -28,12 +28,14 @@ class TestTransitionWrapper : public ::testing::Test } }; -TEST_F(TestTransitionWrapper, empty_transition) { +TEST_F(TestTransitionWrapper, empty_transition) +{ auto a = std::make_shared(1, "my_transition"); EXPECT_NO_THROW(a.reset()); } -TEST_F(TestTransitionWrapper, wrapper) { +TEST_F(TestTransitionWrapper, wrapper) +{ { rclcpp_lifecycle::Transition t(12, "no_states_set"); EXPECT_EQ(12, t.id()); @@ -53,10 +55,7 @@ TEST_F(TestTransitionWrapper, wrapper) { rclcpp_lifecycle::State goal_state(2, "goal_state"); rclcpp_lifecycle::Transition t( - 12, - "from_start_to_goal", - std::move(start_state), - std::move(goal_state)); + 12, "from_start_to_goal", std::move(start_state), std::move(goal_state)); EXPECT_EQ(12, t.id()); EXPECT_FALSE(t.label().empty()); @@ -64,7 +63,8 @@ TEST_F(TestTransitionWrapper, wrapper) { } } -TEST_F(TestTransitionWrapper, copy_constructor) { +TEST_F(TestTransitionWrapper, copy_constructor) +{ auto a = std::make_shared(1, "my_transition"); rclcpp_lifecycle::Transition b(*a); @@ -74,7 +74,8 @@ TEST_F(TestTransitionWrapper, copy_constructor) { EXPECT_STREQ("my_transition", b.label().c_str()); } -TEST_F(TestTransitionWrapper, assignment_operator) { +TEST_F(TestTransitionWrapper, assignment_operator) +{ auto a = std::make_shared(1, "one"); auto b = std::make_shared(2, "two"); *b = *a;