From 9d88f0e9e3559e0aa1d86428b4820c5e0bbdb108 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Tue, 21 Apr 2020 14:54:53 +0200 Subject: [PATCH 001/121] * updated interface to SubscriptionIntraProcessBase * implemented interface to deserialize a given serialized message to ros message Signed-off-by: Joshua Hampp --- .../subscription_intra_process.hpp | 29 +++++++++++++++++++ .../subscription_intra_process_base.hpp | 9 ++++++ 2 files changed, 38 insertions(+) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 618db3cac1..9c7807d12f 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -28,6 +28,8 @@ #include "rclcpp/experimental/buffers/intra_process_buffer.hpp" #include "rclcpp/experimental/create_intra_process_buffer.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" #include "tracetools/tracetools.h" @@ -72,6 +74,12 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase throw std::runtime_error("SubscriptionIntraProcess wrong callback type"); } + if (!allocator) { + message_allocator_ = std::make_shared(); + } else { + message_allocator_ = std::make_shared(*allocator.get()); + } + // Create the intra-process buffer. buffer_ = rclcpp::experimental::create_intra_process_buffer( buffer_type, @@ -102,6 +110,12 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase #endif } + bool + is_serialized() const + { + return rclcpp::is_serialized_message_class::value; + } + bool is_ready(rcl_wait_set_t * wait_set) { @@ -134,6 +148,20 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase return buffer_->use_take_shared_method(); } + void + provide_serialized_intra_process_message(const SerializedMessage & serialized_message) + { + rclcpp::Serialization serialization; + + auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); + MessageAllocTraits::construct(*message_allocator_.get(), ptr); + auto message = MessageUniquePtr(ptr); + + serialization.deserialize_message(serialized_message, *message); + + provide_intra_process_message(std::move(message)); + } + private: void trigger_guard_condition() @@ -168,6 +196,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase AnySubscriptionCallback any_callback_; BufferUniquePtr buffer_; + std::shared_ptr message_allocator_; }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 7afd68abef..97ac402c83 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -25,6 +25,8 @@ #include "rcl/error_handling.h" +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" @@ -70,6 +72,13 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable rmw_qos_profile_t get_actual_qos() const; + virtual bool + is_serialized() const = 0; + + virtual void + provide_serialized_intra_process_message( + const SerializedMessage & serialized_message) = 0; + protected: std::recursive_mutex reentrant_mutex_; rcl_guard_condition_t gc_; From 1dad3ad9610f95d4b1ef6c304057f90e0835c8f4 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Tue, 21 Apr 2020 14:56:30 +0200 Subject: [PATCH 002/121] added support for SerializedMessage in Intraprocessmanger, so: * serialized publisher uses correct method to forward content to subscriber * non-serialized publisher can serialize message for serialized subscriber Signed-off-by: Joshua Hampp --- .../experimental/intra_process_manager.hpp | 97 +++++++++++++++---- 1 file changed, 78 insertions(+), 19 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index d671de701e..d420248f93 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -37,6 +37,8 @@ #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/publisher_base.hpp" +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -356,6 +358,8 @@ class IntraProcessManager std::shared_ptr message, std::vector subscription_ids) { + constexpr bool is_serialized_publisher = rclcpp::is_serialized_message_class::value; + for (auto id : subscription_ids) { auto subscription_it = subscriptions_.find(id); if (subscription_it == subscriptions_.end()) { @@ -363,11 +367,28 @@ class IntraProcessManager } auto subscription_base = subscription_it->second.subscription; - auto subscription = std::static_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcess - >(subscription_base); + // same message type, so it can just be passed through + if (subscription_base->is_serialized() == is_serialized_publisher) { + auto subscription = std::static_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcess + >(subscription_base); + + subscription->provide_intra_process_message(message); + } else if (is_serialized_publisher) { + // publisher provides a serialized message, while subscriber expects a ROS message + provide_serialized_intra_process_message(subscription_base, *message); + } else { + // publisher provides a ROS message, while subscriber expects a serialized message + rclcpp::Serialization serialization; + auto subscription = std::static_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcess + >(subscription_base); + + auto serialized_message = std::make_unique(); + serialization.serialize_message(*message, *serialized_message); - subscription->provide_intra_process_message(message); + subscription->provide_intra_process_message(std::move(serialized_message)); + } } } @@ -384,6 +405,8 @@ class IntraProcessManager using MessageAllocTraits = allocator::AllocRebind; using MessageUniquePtr = std::unique_ptr; + constexpr bool is_serialized_publisher = rclcpp::is_serialized_message_class::value; + for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { auto subscription_it = subscriptions_.find(*it); if (subscription_it == subscriptions_.end()) { @@ -391,26 +414,62 @@ class IntraProcessManager } auto subscription_base = subscription_it->second.subscription; - auto subscription = std::static_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcess - >(subscription_base); - - if (std::next(it) == subscription_ids.end()) { - // If this is the last subscription, give up ownership - subscription->provide_intra_process_message(std::move(message)); + // same message type, so it can just be passed through + if (subscription_base->is_serialized() == is_serialized_publisher) { + auto subscription = std::static_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcess + >(subscription_base); + + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + subscription->provide_intra_process_message(std::move(message)); + } else { + // Copy the message since we have additional subscriptions to serve + MessageUniquePtr copy_message; + Deleter deleter = message.get_deleter(); + auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1); + MessageAllocTraits::construct(*allocator.get(), ptr, *message); + copy_message = MessageUniquePtr(ptr, deleter); + + subscription->provide_intra_process_message(std::move(copy_message)); + } + } else if (is_serialized_publisher) { + // publisher provides a serialized message, while subscriber expects a ROS message + provide_serialized_intra_process_message(subscription_base, *message); } else { - // Copy the message since we have additional subscriptions to serve - MessageUniquePtr copy_message; - Deleter deleter = message.get_deleter(); - auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1); - MessageAllocTraits::construct(*allocator.get(), ptr, *message); - copy_message = MessageUniquePtr(ptr, deleter); - - subscription->provide_intra_process_message(std::move(copy_message)); + // publisher provides a ROS message, while subscriber expects a serialized message + rclcpp::Serialization serialization; + auto subscription = std::static_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcess + >(subscription_base); + + auto serialized_message = std::make_unique(); + serialization.serialize_message(*message, *serialized_message); + + subscription->provide_intra_process_message(std::move(serialized_message)); } } } + template + static std::enable_if_t::value> + provide_serialized_intra_process_message( + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr & subscription, + const T & serialized_message) + { + (void)subscription; + (void)serialized_message; + // prevent call if actual message type does not match + } + + static void + provide_serialized_intra_process_message( + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr & subscription, + const SerializedMessage & serialized_message) + { + subscription->provide_serialized_intra_process_message(serialized_message); + } + PublisherToSubscriptionIdsMap pub_to_subs_; SubscriptionMap subscriptions_; PublisherMap publishers_; From c552c0bef29b71ae2d0e784fe6781a21a5f3dd09 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Tue, 21 Apr 2020 14:56:58 +0200 Subject: [PATCH 003/121] updated unit test for intraprocess manager to match new interface Signed-off-by: Joshua Hampp --- rclcpp/test/test_intra_process_manager.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index b71fb1d061..c6a981f964 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -23,6 +23,8 @@ #define RCLCPP_BUILDING_LIBRARY 1 #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" #include "rclcpp/qos.hpp" #include "rmw/types.h" #include "rmw/qos_profiles.h" @@ -212,6 +214,18 @@ class SubscriptionIntraProcessBase return topic_name; } + bool + is_serialized() const + { + return false; + } + + void + provide_serialized_intra_process_message(const rclcpp::SerializedMessage & serialized_message) + { + (void)serialized_message; + } + rmw_qos_profile_t qos_profile; const char * topic_name; }; From 2513cc2e1599f03c7f383f295105bd2a023ccab2 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Wed, 22 Apr 2020 05:54:32 +0200 Subject: [PATCH 004/121] updated is_serialized_message_class traits Signed-off-by: Joshua Hampp --- .../include/rclcpp/experimental/intra_process_manager.hpp | 8 +++++--- .../rclcpp/experimental/subscription_intra_process.hpp | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index d420248f93..072a53078a 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -358,7 +358,8 @@ class IntraProcessManager std::shared_ptr message, std::vector subscription_ids) { - constexpr bool is_serialized_publisher = rclcpp::is_serialized_message_class::value; + constexpr bool is_serialized_publisher = + serialization_traits::is_serialized_message_class::value; for (auto id : subscription_ids) { auto subscription_it = subscriptions_.find(id); @@ -405,7 +406,8 @@ class IntraProcessManager using MessageAllocTraits = allocator::AllocRebind; using MessageUniquePtr = std::unique_ptr; - constexpr bool is_serialized_publisher = rclcpp::is_serialized_message_class::value; + constexpr bool is_serialized_publisher = + serialization_traits::is_serialized_message_class::value; for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { auto subscription_it = subscriptions_.find(*it); @@ -452,7 +454,7 @@ class IntraProcessManager } template - static std::enable_if_t::value> + static std::enable_if_t::value> provide_serialized_intra_process_message( rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr & subscription, const T & serialized_message) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 9c7807d12f..2b6573287a 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -113,7 +113,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase bool is_serialized() const { - return rclcpp::is_serialized_message_class::value; + return serialization_traits::is_serialized_message_class::value; } bool From fd6a20ec8e2bcc2afed7f84d1a38be8bdcf7f33c Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Wed, 22 Apr 2020 05:58:39 +0200 Subject: [PATCH 005/121] renamed provide_serialized_intra_process_message to provide_intra_process_message Signed-off-by: Joshua Hampp --- .../rclcpp/experimental/intra_process_manager.hpp | 10 +++++----- .../rclcpp/experimental/subscription_intra_process.hpp | 2 +- .../experimental/subscription_intra_process_base.hpp | 2 +- rclcpp/test/test_intra_process_manager.cpp | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 072a53078a..757341d421 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -377,7 +377,7 @@ class IntraProcessManager subscription->provide_intra_process_message(message); } else if (is_serialized_publisher) { // publisher provides a serialized message, while subscriber expects a ROS message - provide_serialized_intra_process_message(subscription_base, *message); + provide_intra_process_message(subscription_base, *message); } else { // publisher provides a ROS message, while subscriber expects a serialized message rclcpp::Serialization serialization; @@ -437,7 +437,7 @@ class IntraProcessManager } } else if (is_serialized_publisher) { // publisher provides a serialized message, while subscriber expects a ROS message - provide_serialized_intra_process_message(subscription_base, *message); + provide_intra_process_message(subscription_base, *message); } else { // publisher provides a ROS message, while subscriber expects a serialized message rclcpp::Serialization serialization; @@ -455,7 +455,7 @@ class IntraProcessManager template static std::enable_if_t::value> - provide_serialized_intra_process_message( + provide_intra_process_message( rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr & subscription, const T & serialized_message) { @@ -465,11 +465,11 @@ class IntraProcessManager } static void - provide_serialized_intra_process_message( + provide_intra_process_message( rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr & subscription, const SerializedMessage & serialized_message) { - subscription->provide_serialized_intra_process_message(serialized_message); + subscription->provide_intra_process_message(serialized_message); } PublisherToSubscriptionIdsMap pub_to_subs_; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 2b6573287a..059ff5e1c8 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -149,7 +149,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase } void - provide_serialized_intra_process_message(const SerializedMessage & serialized_message) + provide_intra_process_message(const SerializedMessage & serialized_message) { rclcpp::Serialization serialization; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 97ac402c83..84af55fa0c 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -76,7 +76,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable is_serialized() const = 0; virtual void - provide_serialized_intra_process_message( + provide_intra_process_message( const SerializedMessage & serialized_message) = 0; protected: diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index c6a981f964..f7419e08f3 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -221,7 +221,7 @@ class SubscriptionIntraProcessBase } void - provide_serialized_intra_process_message(const rclcpp::SerializedMessage & serialized_message) + provide_intra_process_message(const rclcpp::SerializedMessage & serialized_message) { (void)serialized_message; } From f0c8dd31c3b1782e2a17d229553bac178c26a489 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Wed, 22 Apr 2020 06:06:35 +0200 Subject: [PATCH 006/121] fixed Serialization class for non-supported type (yes this is actually needed for the next PR) Signed-off-by: Joshua Hampp --- rclcpp/include/rclcpp/serialization.hpp | 28 +++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/rclcpp/include/rclcpp/serialization.hpp b/rclcpp/include/rclcpp/serialization.hpp index 53fe59dc8b..9666a81a5e 100644 --- a/rclcpp/include/rclcpp/serialization.hpp +++ b/rclcpp/include/rclcpp/serialization.hpp @@ -97,6 +97,34 @@ class Serialization : public SerializationBase } }; +/// Specialized serialization for rcl_serialized_message_t (because type support is not defined) +template<> +class Serialization: public SerializationBase +{ +public: + /// Constructor of Serialization + Serialization() + : SerializationBase(nullptr) + { + throw std::runtime_error( + "Serialization of rcl_serialized_message_t to serialized message is not possible."); + } +}; + +/// Specialized serialization for SerializedMessage (because type support is not defined) +template<> +class Serialization: public SerializationBase +{ +public: + /// Constructor of Serialization + Serialization() + : SerializationBase(nullptr) + { + throw std::runtime_error( + "Serialization of SerializedMessage to serialized message is not possible."); + } +}; + } // namespace rclcpp #endif // RCLCPP__SERIALIZATION_HPP_ From facb43d7df4069a4ae8cab951f408e4d3bd1f681 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Wed, 22 Apr 2020 06:06:59 +0200 Subject: [PATCH 007/121] updated to (de)seriaizle method Signed-off-by: Joshua Hampp --- .../include/rclcpp/experimental/intra_process_manager.hpp | 6 ++++-- .../rclcpp/experimental/subscription_intra_process.hpp | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 757341d421..a2d765ce64 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -386,7 +386,8 @@ class IntraProcessManager >(subscription_base); auto serialized_message = std::make_unique(); - serialization.serialize_message(*message, *serialized_message); + serialization.serialize_message( + reinterpret_cast(message.get()), serialized_message.get()); subscription->provide_intra_process_message(std::move(serialized_message)); } @@ -446,7 +447,8 @@ class IntraProcessManager >(subscription_base); auto serialized_message = std::make_unique(); - serialization.serialize_message(*message, *serialized_message); + serialization.serialize_message( + reinterpret_cast(message.get()), serialized_message.get()); subscription->provide_intra_process_message(std::move(serialized_message)); } diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 059ff5e1c8..e7af4cef54 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -157,7 +157,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase MessageAllocTraits::construct(*message_allocator_.get(), ptr); auto message = MessageUniquePtr(ptr); - serialization.deserialize_message(serialized_message, *message); + serialization.deserialize_message(&serialized_message, reinterpret_cast(message.get())); provide_intra_process_message(std::move(message)); } From 08a3251e83627f56e8711696d9a4dd5694cb5121 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 21 Apr 2020 08:57:31 -0700 Subject: [PATCH 008/121] fix build regression (#1078) Signed-off-by: Dirk Thomas --- rclcpp/include/rclcpp/node_interfaces/node_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index c21514ff97..9816325bff 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -43,7 +43,7 @@ class NodeBase : public NodeBaseInterface rclcpp::Context::SharedPtr context, const rcl_node_options_t & rcl_node_options, bool use_intra_process_default, - bool enable_topic_statistics_default); + bool enable_topic_statistics_default=false); RCLCPP_PUBLIC virtual From 0577f390242de78eeec1ae75fda9be7193995e47 Mon Sep 17 00:00:00 2001 From: Prajakta Gokhale Date: Tue, 21 Apr 2020 14:14:47 -0700 Subject: [PATCH 009/121] Reflect changes in rclcpp API (#1079) * Reflect changes in rclcpp API Signed-off-by: Prajakta Gokhale * Revert earlier fix made in rclcpp Signed-off-by: Prajakta Gokhale --- rclcpp/include/rclcpp/node_interfaces/node_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 9816325bff..c21514ff97 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -43,7 +43,7 @@ class NodeBase : public NodeBaseInterface rclcpp::Context::SharedPtr context, const rcl_node_options_t & rcl_node_options, bool use_intra_process_default, - bool enable_topic_statistics_default=false); + bool enable_topic_statistics_default); RCLCPP_PUBLIC virtual From 0bf988aa62adcd41b8557cb64be68900b00aa56b Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Wed, 29 Apr 2020 09:25:59 +0200 Subject: [PATCH 010/121] * added get_type_support_handle to rclcpp::Serialization * removed specialization to rclcpp::Serialization Signed-off-by: Joshua Hampp --- rclcpp/include/rclcpp/serialization.hpp | 69 ++++++++++++++----------- 1 file changed, 38 insertions(+), 31 deletions(-) diff --git a/rclcpp/include/rclcpp/serialization.hpp b/rclcpp/include/rclcpp/serialization.hpp index 9666a81a5e..bc9063a9b8 100644 --- a/rclcpp/include/rclcpp/serialization.hpp +++ b/rclcpp/include/rclcpp/serialization.hpp @@ -48,6 +48,32 @@ struct is_serialized_message_class: std::true_type {}; } // namespace serialization_traits +namespace serialization +{ +template +inline const rosidl_message_type_support_t * +get_type_support_handle_impl() +{ + return rosidl_typesupport_cpp::get_message_type_support_handle(); +} + +// no message type support for rclcpp::SerializedMessage +template<> +inline const rosidl_message_type_support_t * +get_type_support_handle_impl() +{ + return nullptr; +} + +// no message type support for rcl_serialized_message_t +template<> +inline const rosidl_message_type_support_t * +get_type_support_handle_impl() +{ + return nullptr; +} +} // namespace serialization + /// Interface to (de)serialize a message class RCLCPP_PUBLIC_TYPE SerializationBase { @@ -78,6 +104,13 @@ class RCLCPP_PUBLIC_TYPE SerializationBase void deserialize_message( const SerializedMessage * serialized_message, void * ros_message) const; + /// Get the message type support for the serialized message + /** + * \return The message type support. + */ + virtual const rosidl_message_type_support_t * + get_type_support_handle() const = 0; + private: const rosidl_message_type_support_t * type_support_; }; @@ -89,39 +122,13 @@ class Serialization : public SerializationBase public: /// Constructor of Serialization Serialization() - : SerializationBase(rosidl_typesupport_cpp::get_message_type_support_handle()) - { - static_assert( - !serialization_traits::is_serialized_message_class::value, - "Serialization of serialized message to serialized message is not possible."); - } -}; - -/// Specialized serialization for rcl_serialized_message_t (because type support is not defined) -template<> -class Serialization: public SerializationBase -{ -public: - /// Constructor of Serialization - Serialization() - : SerializationBase(nullptr) - { - throw std::runtime_error( - "Serialization of rcl_serialized_message_t to serialized message is not possible."); - } -}; + : SerializationBase(get_type_support_handle()) + {} -/// Specialized serialization for SerializedMessage (because type support is not defined) -template<> -class Serialization: public SerializationBase -{ -public: - /// Constructor of Serialization - Serialization() - : SerializationBase(nullptr) + const rosidl_message_type_support_t * + get_type_support_handle() const override { - throw std::runtime_error( - "Serialization of SerializedMessage to serialized message is not possible."); + return serialization::get_type_support_handle_impl(); } }; From 7a731799277780234d1585717dac75a89767ec42 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Wed, 29 Apr 2020 09:29:37 +0200 Subject: [PATCH 011/121] extended subscription_intra_process(_base): * allocate message * getter for serialization * provide untyped message Signed-off-by: Joshua Hampp --- .../subscription_intra_process.hpp | 34 ++++++++++++------- .../subscription_intra_process_base.hpp | 13 +++++-- 2 files changed, 33 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index e7af4cef54..4ce8e48da5 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -129,10 +129,9 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase } void - provide_intra_process_message(ConstMessageSharedPtr message) + provide_intra_process_message(std::shared_ptr message) { - buffer_->add_shared(std::move(message)); - trigger_guard_condition(); + provide_intra_process_message_impl(std::static_pointer_cast(message)); } void @@ -148,18 +147,22 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase return buffer_->use_take_shared_method(); } - void - provide_intra_process_message(const SerializedMessage & serialized_message) + std::shared_ptr + create_shared_message(const void * message_to_copy) { - rclcpp::Serialization serialization; - - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr); - auto message = MessageUniquePtr(ptr); + if (nullptr != message_to_copy) { + return std::allocate_shared( + *message_allocator_, + *reinterpret_cast(message_to_copy)); + } - serialization.deserialize_message(&serialized_message, reinterpret_cast(message.get())); + return std::allocate_shared(*message_allocator_); + } - provide_intra_process_message(std::move(message)); + std::unique_ptr + get_serialization() + { + return std::make_unique>(); } private: @@ -170,6 +173,13 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase (void)ret; } + void + provide_intra_process_message_impl(ConstMessageSharedPtr message) + { + buffer_->add_shared(std::move(message)); + trigger_guard_condition(); + } + template typename std::enable_if::value, void>::type execute_impl() diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 84af55fa0c..a7fae5bac2 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -72,12 +72,21 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable rmw_qos_profile_t get_actual_qos() const; + RCLCPP_PUBLIC virtual bool is_serialized() const = 0; + RCLCPP_PUBLIC virtual void - provide_intra_process_message( - const SerializedMessage & serialized_message) = 0; + provide_intra_process_message(std::shared_ptr message) = 0; + + RCLCPP_PUBLIC + virtual std::shared_ptr + create_shared_message(const void * message_to_copy = nullptr) = 0; + + RCLCPP_PUBLIC + virtual std::unique_ptr + get_serialization() = 0; protected: std::recursive_mutex reentrant_mutex_; From 167ae5f6cca46db5c8707c7b647c2f5367902681 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Wed, 29 Apr 2020 09:32:19 +0200 Subject: [PATCH 012/121] update unit test for subscription_intra_process(_base) Signed-off-by: Joshua Hampp --- rclcpp/test/test_intra_process_manager.cpp | 32 +++++++++++++++++----- 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index f7419e08f3..369df01b7f 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -220,11 +220,14 @@ class SubscriptionIntraProcessBase return false; } - void - provide_intra_process_message(const rclcpp::SerializedMessage & serialized_message) - { - (void)serialized_message; - } + virtual void + provide_intra_process_message(std::shared_ptr message) = 0; + + virtual std::shared_ptr + create_shared_message(const void * message_to_copy = nullptr) = 0; + + virtual std::unique_ptr + get_serialization() = 0; rmw_qos_profile_t qos_profile; const char * topic_name; @@ -243,9 +246,9 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase } void - provide_intra_process_message(std::shared_ptr msg) + provide_intra_process_message(std::shared_ptr message) { - buffer->add(msg); + buffer->add(std::static_pointer_cast(message)); } void @@ -254,6 +257,21 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase buffer->add(std::move(msg)); } + virtual std::shared_ptr + create_shared_message(const void * message_to_copy = nullptr) + { + if (nullptr != message_to_copy) { + return std::make_shared(*reinterpret_cast(message_to_copy)); + } + return std::make_shared(); + } + + virtual std::unique_ptr + get_serialization() + { + return std::make_unique>(); + } + std::uintptr_t pop() { From 372df2a6541a6da050ca501126c1738a6d79c69d Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Wed, 29 Apr 2020 09:35:37 +0200 Subject: [PATCH 013/121] extended intra_process_manager for serialized messages: * two extra subscription/publisher pairs (for serialized ones) * added methods for (de)serialization * added methods for messages without known allocator * splitted up publishing with same type and not matching message type Signed-off-by: Joshua Hampp --- .../experimental/intra_process_manager.hpp | 453 ++++++++++++------ rclcpp/src/rclcpp/intra_process_manager.cpp | 47 +- 2 files changed, 338 insertions(+), 162 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index a2d765ce64..d80acc2cf5 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -41,6 +41,8 @@ #include "rclcpp/serialized_message.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcpputils/asserts.hpp" + namespace rclcpp { @@ -185,8 +187,21 @@ class IntraProcessManager std::unique_ptr message, std::shared_ptr::allocator_type> allocator) { - using MessageAllocTraits = allocator::AllocRebind; - using MessageAllocatorT = typename MessageAllocTraits::allocator_type; + constexpr bool is_serialized_publisher = + serialization_traits::is_serialized_message_class::value; + + constexpr auto index_ownership_same = SplittedSubscriptions::get_index( + false, + is_serialized_publisher); + constexpr auto index_shared_same = SplittedSubscriptions::get_index( + true, + is_serialized_publisher); + constexpr auto index_ownership_other = SplittedSubscriptions::get_index( + false, + !is_serialized_publisher); + constexpr auto index_shared_other = SplittedSubscriptions::get_index( + true, + !is_serialized_publisher); std::shared_lock lock(mutex_); @@ -200,40 +215,22 @@ class IntraProcessManager } const auto & sub_ids = publisher_it->second; - if (sub_ids.take_ownership_subscriptions.empty()) { - // None of the buffers require ownership, so we promote the pointer - std::shared_ptr msg = std::move(message); - - this->template add_shared_msg_to_buffers(msg, sub_ids.take_shared_subscriptions); - } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT - sub_ids.take_shared_subscriptions.size() <= 1) - { - // There is at maximum 1 buffer that does not require ownership. - // So we this case is equivalent to all the buffers requiring ownership - - // Merge the two vector of ids into a unique one - std::vector concatenated_vector(sub_ids.take_shared_subscriptions); - concatenated_vector.insert( - concatenated_vector.end(), - sub_ids.take_ownership_subscriptions.begin(), - sub_ids.take_ownership_subscriptions.end()); - - this->template add_owned_msg_to_buffers( - std::move(message), - concatenated_vector, - allocator); - } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT - sub_ids.take_shared_subscriptions.size() > 1) + // check if (de)serialization is needed + if (sub_ids.take_subscriptions[index_ownership_other].size() + + sub_ids.take_subscriptions[index_shared_other].size() > 0) { - // Construct a new shared pointer from the message - // for the buffers that do not require ownership - auto shared_msg = std::allocate_shared(*allocator, *message); - - this->template add_shared_msg_to_buffers( - shared_msg, sub_ids.take_shared_subscriptions); - this->template add_owned_msg_to_buffers( - std::move(message), sub_ids.take_ownership_subscriptions, allocator); + do_intra_process_publish_other_type( + message.get(), + sub_ids.take_subscriptions[index_ownership_other], + sub_ids.take_subscriptions[index_shared_other] + ); } + + do_intra_process_publish_same_type( + std::move(message), allocator, + sub_ids.take_subscriptions[index_ownership_same], + sub_ids.take_subscriptions[index_shared_same] + ); } template< @@ -246,8 +243,21 @@ class IntraProcessManager std::unique_ptr message, std::shared_ptr::allocator_type> allocator) { - using MessageAllocTraits = allocator::AllocRebind; - using MessageAllocatorT = typename MessageAllocTraits::allocator_type; + constexpr bool is_serialized_publisher = + serialization_traits::is_serialized_message_class::value; + + constexpr auto index_ownership_same = SplittedSubscriptions::get_index( + false, + is_serialized_publisher); + constexpr auto index_shared_same = SplittedSubscriptions::get_index( + true, + is_serialized_publisher); + constexpr auto index_ownership_other = SplittedSubscriptions::get_index( + false, + !is_serialized_publisher); + constexpr auto index_shared_other = SplittedSubscriptions::get_index( + true, + !is_serialized_publisher); std::shared_lock lock(mutex_); @@ -261,33 +271,22 @@ class IntraProcessManager } const auto & sub_ids = publisher_it->second; - if (sub_ids.take_ownership_subscriptions.empty()) { - // If there are no owning, just convert to shared. - std::shared_ptr shared_msg = std::move(message); - if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( - shared_msg, sub_ids.take_shared_subscriptions); - } - return shared_msg; - } else { - // Construct a new shared pointer from the message for the buffers that - // do not require ownership and to return. - auto shared_msg = std::allocate_shared(*allocator, *message); - - if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( - shared_msg, - sub_ids.take_shared_subscriptions); - } - if (!sub_ids.take_ownership_subscriptions.empty()) { - this->template add_owned_msg_to_buffers( - std::move(message), - sub_ids.take_ownership_subscriptions, - allocator); - } - - return shared_msg; + // check if (de)serialization is needed + if (sub_ids.take_subscriptions[index_ownership_other].size() + + sub_ids.take_subscriptions[index_shared_other].size() > 0) + { + do_intra_process_publish_other_type( + message.get(), + sub_ids.take_subscriptions[index_ownership_other], + sub_ids.take_subscriptions[index_shared_other] + ); } + + return do_intra_process_publish_and_return_shared_same_type( + std::move(message), allocator, + sub_ids.take_subscriptions[index_ownership_same], + sub_ids.take_subscriptions[index_shared_same] + ); } /// Return true if the given rmw_gid_t matches any stored Publishers. @@ -326,8 +325,22 @@ class IntraProcessManager struct SplittedSubscriptions { - std::vector take_shared_subscriptions; - std::vector take_ownership_subscriptions; + enum + { + IndexSharedTyped = 0, IndexSharedSerialized = 1, + IndexOwnershipTyped = 2, IndexOwnershipSerialized = 3, + IndexNum = 4 + }; + + /// get the index for "take_subscriptions" depending on shared/serialized + constexpr static auto + get_index(const bool is_shared, const bool is_serialized) + { + return (is_serialized ? IndexSharedTyped : IndexSharedSerialized) + + (is_shared ? IndexSharedTyped : IndexOwnershipTyped); + } + + std::vector take_subscriptions[IndexNum]; }; using SubscriptionMap = @@ -346,21 +359,165 @@ class IntraProcessManager RCLCPP_PUBLIC void - insert_sub_id_for_pub(uint64_t sub_id, uint64_t pub_id, bool use_take_shared_method); + insert_sub_id_for_pub( + uint64_t sub_id, uint64_t pub_id, bool use_take_shared_method, + bool is_serialized_subscriber); RCLCPP_PUBLIC bool can_communicate(PublisherInfo pub_info, SubscriptionInfo sub_info) const; + template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> + void + do_intra_process_publish_same_type( + std::unique_ptr message, + std::shared_ptr::allocator_type> allocator, + const std::vector & take_ownership_subscriptions, + const std::vector & take_shared_subscriptions) + { + using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocatorT = typename MessageAllocTraits::allocator_type; + + if (take_ownership_subscriptions.empty()) { + // None of the buffers require ownership, so we promote the pointer + std::shared_ptr msg = std::move(message); + + this->template add_shared_msg_to_buffers(msg, take_shared_subscriptions); + } else if (!take_ownership_subscriptions.empty() && // NOLINT + take_shared_subscriptions.size() <= 1) + { + // There is at maximum 1 buffer that does not require ownership. + // So we this case is equivalent to all the buffers requiring ownership + + // Merge the two vector of ids into a unique one + std::vector concatenated_vector(take_shared_subscriptions); + concatenated_vector.insert( + concatenated_vector.end(), + take_ownership_subscriptions.begin(), + take_ownership_subscriptions.end()); + + this->template add_owned_msg_to_buffers( + std::move(message), + concatenated_vector, + allocator); + } else if (!take_ownership_subscriptions.empty() && // NOLINT + take_shared_subscriptions.size() > 1) + { + // Construct a new shared pointer from the message + // for the buffers that do not require ownership + auto shared_msg = std::allocate_shared(*allocator, *message); + + this->template add_shared_msg_to_buffers( + shared_msg, take_shared_subscriptions); + this->template add_owned_msg_to_buffers( + std::move(message), take_ownership_subscriptions, allocator); + } + } + + template + void + do_intra_process_publish_other_type( + const MessageT * unsupported_message, + const std::vector & take_ownership_subscriptions, + const std::vector & take_shared_subscriptions) + { + // get first subscription + const auto subscription_id = + take_ownership_subscriptions.empty() ? + take_shared_subscriptions.front() : + take_ownership_subscriptions.front(); + + auto subscription_it = subscriptions_.find(subscription_id); + if (subscription_it == subscriptions_.end()) { + throw std::runtime_error("subscription has unexpectedly gone out of scope"); + } + auto subscription_base = subscription_it->second.subscription; + + // convert published message to the supported type + auto message = convert_message(unsupported_message, subscription_base); + + if (take_ownership_subscriptions.empty()) { + // None of the buffers require ownership, so we promote the pointer + this->template add_shared_msg_to_buffers(std::move(message), take_shared_subscriptions); + } else if (!take_ownership_subscriptions.empty() && // NOLINT + take_shared_subscriptions.size() <= 1) + { + // There is at maximum 1 buffer that does not require ownership. + // So we this case is equivalent to all the buffers requiring ownership + + // Merge the two vector of ids into a unique one + std::vector concatenated_vector(take_shared_subscriptions); + concatenated_vector.insert( + concatenated_vector.end(), + take_ownership_subscriptions.begin(), + take_ownership_subscriptions.end()); + + add_owned_msg_to_buffers(std::move(message), concatenated_vector); + } else if (!take_ownership_subscriptions.empty() && // NOLINT + take_shared_subscriptions.size() > 1) + { + // Construct a new shared pointer from the message + // for the buffers that do not require ownership + auto shared_msg = subscription_base->create_shared_message(message.get()); + + this->template add_shared_msg_to_buffers( + shared_msg, take_shared_subscriptions); + add_owned_msg_to_buffers(std::move(message), take_ownership_subscriptions); + } + } + + template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> + std::shared_ptr + do_intra_process_publish_and_return_shared_same_type( + std::unique_ptr message, + std::shared_ptr::allocator_type> allocator, + const std::vector & take_ownership_subscriptions, + const std::vector & take_shared_subscriptions) + { + using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocatorT = typename MessageAllocTraits::allocator_type; + + if (take_ownership_subscriptions.empty()) { + // If there are no owning, just convert to shared. + std::shared_ptr shared_msg = std::move(message); + if (!take_shared_subscriptions.empty()) { + this->template add_shared_msg_to_buffers( + shared_msg, take_shared_subscriptions); + } + return shared_msg; + } else { + // Construct a new shared pointer from the message for the buffers that + // do not require ownership and to return. + auto shared_msg = std::allocate_shared(*allocator, *message); + + if (!take_shared_subscriptions.empty()) { + this->template add_shared_msg_to_buffers( + shared_msg, + take_shared_subscriptions); + } + if (!take_ownership_subscriptions.empty()) { + this->template add_owned_msg_to_buffers( + std::move(message), + take_ownership_subscriptions, + allocator); + } + + return shared_msg; + } + } + template void add_shared_msg_to_buffers( std::shared_ptr message, std::vector subscription_ids) { - constexpr bool is_serialized_publisher = - serialization_traits::is_serialized_message_class::value; - for (auto id : subscription_ids) { auto subscription_it = subscriptions_.find(id); if (subscription_it == subscriptions_.end()) { @@ -368,29 +525,7 @@ class IntraProcessManager } auto subscription_base = subscription_it->second.subscription; - // same message type, so it can just be passed through - if (subscription_base->is_serialized() == is_serialized_publisher) { - auto subscription = std::static_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcess - >(subscription_base); - - subscription->provide_intra_process_message(message); - } else if (is_serialized_publisher) { - // publisher provides a serialized message, while subscriber expects a ROS message - provide_intra_process_message(subscription_base, *message); - } else { - // publisher provides a ROS message, while subscriber expects a serialized message - rclcpp::Serialization serialization; - auto subscription = std::static_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcess - >(subscription_base); - - auto serialized_message = std::make_unique(); - serialization.serialize_message( - reinterpret_cast(message.get()), serialized_message.get()); - - subscription->provide_intra_process_message(std::move(serialized_message)); - } + subscription_base->provide_intra_process_message(message); } } @@ -407,9 +542,6 @@ class IntraProcessManager using MessageAllocTraits = allocator::AllocRebind; using MessageUniquePtr = std::unique_ptr; - constexpr bool is_serialized_publisher = - serialization_traits::is_serialized_message_class::value; - for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { auto subscription_it = subscriptions_.find(*it); if (subscription_it == subscriptions_.end()) { @@ -417,61 +549,106 @@ class IntraProcessManager } auto subscription_base = subscription_it->second.subscription; - // same message type, so it can just be passed through - if (subscription_base->is_serialized() == is_serialized_publisher) { - auto subscription = std::static_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcess - >(subscription_base); - - if (std::next(it) == subscription_ids.end()) { - // If this is the last subscription, give up ownership - subscription->provide_intra_process_message(std::move(message)); - } else { - // Copy the message since we have additional subscriptions to serve - MessageUniquePtr copy_message; - Deleter deleter = message.get_deleter(); - auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1); - MessageAllocTraits::construct(*allocator.get(), ptr, *message); - copy_message = MessageUniquePtr(ptr, deleter); - - subscription->provide_intra_process_message(std::move(copy_message)); - } - } else if (is_serialized_publisher) { - // publisher provides a serialized message, while subscriber expects a ROS message - provide_intra_process_message(subscription_base, *message); + auto subscription = std::static_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcess + >(subscription_base); + + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + subscription->provide_intra_process_message(std::move(message)); } else { - // publisher provides a ROS message, while subscriber expects a serialized message - rclcpp::Serialization serialization; - auto subscription = std::static_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcess - >(subscription_base); + // Copy the message since we have additional subscriptions to serve + MessageUniquePtr copy_message; + Deleter deleter = message.get_deleter(); + auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1); + MessageAllocTraits::construct(*allocator.get(), ptr, *message); + copy_message = MessageUniquePtr(ptr, deleter); + + subscription->provide_intra_process_message(std::move(copy_message)); + } + } + } - auto serialized_message = std::make_unique(); - serialization.serialize_message( - reinterpret_cast(message.get()), serialized_message.get()); + /// method for unknown allocator using subscription for allocation + void + add_owned_msg_to_buffers( + std::shared_ptr message, + std::vector subscription_ids) + { + for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { + auto subscription_it = subscriptions_.find(*it); + if (subscription_it == subscriptions_.end()) { + throw std::runtime_error("subscription has unexpectedly gone out of scope"); + } + auto subscription_base = subscription_it->second.subscription; + + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + subscription_base->provide_intra_process_message(std::move(message)); + } else { + // Copy the message since we have additional subscriptions to serve + std::shared_ptr copy_message = + subscription_base->create_shared_message(message.get()); - subscription->provide_intra_process_message(std::move(serialized_message)); + subscription_base->provide_intra_process_message(std::move(copy_message)); } } } - template - static std::enable_if_t::value> - provide_intra_process_message( - rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr & subscription, - const T & serialized_message) + /// Convert received message to message type of subscriber + /** + * Method for ros message for serialization to rclcpp::SerializedMessage. + * The publisher has a template type while the subscribers is serialized. + * + * In addition this generates a unique intra process id for the publisher. + * + * \param message the ros message from publisher. + * \param subscription a subscriber used for creating the serialized message. + * \return a shared pointer (containing rclcpp::SerializedMessage) with deleter. + */ + template + std::shared_ptr convert_message( + const MessageT * message, + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription) { - (void)subscription; - (void)serialized_message; - // prevent call if actual message type does not match + // serialize + auto serialized_message = subscription->create_shared_message(); + rclcpp::Serialization serialization; + + rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer."); + + serialization.serialize_message( + message, + reinterpret_cast(serialized_message.get())); + + return serialized_message; } - static void - provide_intra_process_message( - rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr & subscription, - const SerializedMessage & serialized_message) + /// Convert received message to message type of subscriber + /** + * Overloaded method for rclcpp::SerializedMessage for deserialization. + * The publisher is serialized while the subscribers have a templated message type. + * + * In addition this generates a unique intra process id for the publisher. + * + * \param serialized_message the serialized message from publisher. + * \param subscription a subscriber used for creating ros message and serialization. + * \return a shared pointer (containing a ros message) with deleter. + */ + std::shared_ptr convert_message( + const SerializedMessage * serialized_message, + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription) { - subscription->provide_intra_process_message(serialized_message); + // deserialize + auto converted_message = subscription->create_shared_message(); + auto serialization = subscription->get_serialization(); + + rcpputils::check_true(nullptr != converted_message, "Converted message is nullpointer."); + rcpputils::check_true(nullptr != serialization, "Serialization is nullpointer."); + + serialization->deserialize_message(serialized_message, converted_message.get()); + + return converted_message; } PublisherToSubscriptionIdsMap pub_to_subs_; diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index 0b9c9d6670..876badc069 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -48,7 +48,9 @@ IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher) // create an entry for the publisher id and populate with already existing subscriptions for (auto & pair : subscriptions_) { if (can_communicate(publishers_[id], pair.second)) { - insert_sub_id_for_pub(pair.first, id, pair.second.use_take_shared_method); + insert_sub_id_for_pub( + pair.first, id, pair.second.use_take_shared_method, + pair.second.subscription->is_serialized()); } } @@ -70,7 +72,9 @@ IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr su // adds the subscription id to all the matchable publishers for (auto & pair : publishers_) { if (can_communicate(pair.second, subscriptions_[id])) { - insert_sub_id_for_pub(id, pair.first, subscriptions_[id].use_take_shared_method); + insert_sub_id_for_pub( + id, pair.first, subscriptions_[id].use_take_shared_method, + subscription->is_serialized()); } } @@ -85,19 +89,14 @@ IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) subscriptions_.erase(intra_process_subscription_id); for (auto & pair : pub_to_subs_) { - pair.second.take_shared_subscriptions.erase( - std::remove( - pair.second.take_shared_subscriptions.begin(), - pair.second.take_shared_subscriptions.end(), - intra_process_subscription_id), - pair.second.take_shared_subscriptions.end()); - - pair.second.take_ownership_subscriptions.erase( - std::remove( - pair.second.take_ownership_subscriptions.begin(), - pair.second.take_ownership_subscriptions.end(), - intra_process_subscription_id), - pair.second.take_ownership_subscriptions.end()); + for (auto i = 0u; i < SplittedSubscriptions::IndexNum; ++i) { + pair.second.take_subscriptions[i].erase( + std::remove( + pair.second.take_subscriptions[i].begin(), + pair.second.take_subscriptions[i].end(), + intra_process_subscription_id), + pair.second.take_subscriptions[i].end()); + } } } @@ -141,9 +140,10 @@ IntraProcessManager::get_subscription_count(uint64_t intra_process_publisher_id) return 0; } - auto count = - publisher_it->second.take_shared_subscriptions.size() + - publisher_it->second.take_ownership_subscriptions.size(); + auto count = 0u; + for (auto i = 0u; i < SplittedSubscriptions::IndexNum; ++i) { + count += publisher_it->second.take_subscriptions[i].size(); + } return count; } @@ -187,13 +187,12 @@ void IntraProcessManager::insert_sub_id_for_pub( uint64_t sub_id, uint64_t pub_id, - bool use_take_shared_method) + bool use_take_shared_method, + bool is_serialized_subscriber) { - if (use_take_shared_method) { - pub_to_subs_[pub_id].take_shared_subscriptions.push_back(sub_id); - } else { - pub_to_subs_[pub_id].take_ownership_subscriptions.push_back(sub_id); - } + pub_to_subs_[pub_id].take_subscriptions[SplittedSubscriptions::get_index( + use_take_shared_method, + is_serialized_subscriber)].push_back(sub_id); } bool From a771a224ea8b85457ef5b0b68730b14cd4272738 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Wed, 29 Apr 2020 18:36:08 +0200 Subject: [PATCH 014/121] simplified code Signed-off-by: Joshua Hampp --- .../experimental/intra_process_manager.hpp | 68 +++++++++---------- 1 file changed, 31 insertions(+), 37 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index d80acc2cf5..f4e800965f 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -190,18 +190,7 @@ class IntraProcessManager constexpr bool is_serialized_publisher = serialization_traits::is_serialized_message_class::value; - constexpr auto index_ownership_same = SplittedSubscriptions::get_index( - false, - is_serialized_publisher); - constexpr auto index_shared_same = SplittedSubscriptions::get_index( - true, - is_serialized_publisher); - constexpr auto index_ownership_other = SplittedSubscriptions::get_index( - false, - !is_serialized_publisher); - constexpr auto index_shared_other = SplittedSubscriptions::get_index( - true, - !is_serialized_publisher); + using Indicies = SplittedSubscriptionsIndicies; std::shared_lock lock(mutex_); @@ -216,20 +205,20 @@ class IntraProcessManager const auto & sub_ids = publisher_it->second; // check if (de)serialization is needed - if (sub_ids.take_subscriptions[index_ownership_other].size() + - sub_ids.take_subscriptions[index_shared_other].size() > 0) + if (sub_ids.take_subscriptions[Indicies::ownership_other].size() + + sub_ids.take_subscriptions[Indicies::shared_other].size() > 0) { do_intra_process_publish_other_type( message.get(), - sub_ids.take_subscriptions[index_ownership_other], - sub_ids.take_subscriptions[index_shared_other] + sub_ids.take_subscriptions[Indicies::ownership_other], + sub_ids.take_subscriptions[Indicies::shared_other] ); } do_intra_process_publish_same_type( std::move(message), allocator, - sub_ids.take_subscriptions[index_ownership_same], - sub_ids.take_subscriptions[index_shared_same] + sub_ids.take_subscriptions[Indicies::ownership_same], + sub_ids.take_subscriptions[Indicies::shared_same] ); } @@ -245,19 +234,7 @@ class IntraProcessManager { constexpr bool is_serialized_publisher = serialization_traits::is_serialized_message_class::value; - - constexpr auto index_ownership_same = SplittedSubscriptions::get_index( - false, - is_serialized_publisher); - constexpr auto index_shared_same = SplittedSubscriptions::get_index( - true, - is_serialized_publisher); - constexpr auto index_ownership_other = SplittedSubscriptions::get_index( - false, - !is_serialized_publisher); - constexpr auto index_shared_other = SplittedSubscriptions::get_index( - true, - !is_serialized_publisher); + using Indicies = SplittedSubscriptionsIndicies; std::shared_lock lock(mutex_); @@ -272,20 +249,20 @@ class IntraProcessManager const auto & sub_ids = publisher_it->second; // check if (de)serialization is needed - if (sub_ids.take_subscriptions[index_ownership_other].size() + - sub_ids.take_subscriptions[index_shared_other].size() > 0) + if (sub_ids.take_subscriptions[Indicies::ownership_other].size() + + sub_ids.take_subscriptions[Indicies::shared_other].size() > 0) { do_intra_process_publish_other_type( message.get(), - sub_ids.take_subscriptions[index_ownership_other], - sub_ids.take_subscriptions[index_shared_other] + sub_ids.take_subscriptions[Indicies::ownership_other], + sub_ids.take_subscriptions[Indicies::shared_other] ); } return do_intra_process_publish_and_return_shared_same_type( std::move(message), allocator, - sub_ids.take_subscriptions[index_ownership_same], - sub_ids.take_subscriptions[index_shared_same] + sub_ids.take_subscriptions[Indicies::ownership_same], + sub_ids.take_subscriptions[Indicies::shared_same] ); } @@ -343,6 +320,23 @@ class IntraProcessManager std::vector take_subscriptions[IndexNum]; }; + template + struct SplittedSubscriptionsIndicies + { + constexpr static auto ownership_same = SplittedSubscriptions::get_index( + false, + is_serialized); + constexpr static auto shared_same = SplittedSubscriptions::get_index( + true, + is_serialized); + constexpr static auto ownership_other = SplittedSubscriptions::get_index( + false, + !is_serialized); + constexpr static auto shared_other = SplittedSubscriptions::get_index( + true, + !is_serialized); + }; + using SubscriptionMap = std::unordered_map; From 71bcd0787e75352964cc0f2aa7a40d60e2c60837 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Wed, 29 Apr 2020 18:36:18 +0200 Subject: [PATCH 015/121] fixed modifier Signed-off-by: Joshua Hampp --- rclcpp/include/rclcpp/experimental/intra_process_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index f4e800965f..1da71f5ed3 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -566,7 +566,7 @@ class IntraProcessManager /// method for unknown allocator using subscription for allocation void add_owned_msg_to_buffers( - std::shared_ptr message, + std::shared_ptr message, std::vector subscription_ids) { for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { From 4480d4e3a0f5562c90ebe41a49562f9632ebfec5 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Wed, 29 Apr 2020 18:36:41 +0200 Subject: [PATCH 016/121] updated comments Signed-off-by: Joshua Hampp --- .../experimental/intra_process_manager.hpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 1da71f5ed3..39a5d16aa1 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -189,7 +189,6 @@ class IntraProcessManager { constexpr bool is_serialized_publisher = serialization_traits::is_serialized_message_class::value; - using Indicies = SplittedSubscriptionsIndicies; std::shared_lock lock(mutex_); @@ -372,6 +371,7 @@ class IntraProcessManager const std::vector & take_ownership_subscriptions, const std::vector & take_shared_subscriptions) { + // subsriber and publisher have same message types using MessageAllocTraits = allocator::AllocRebind; using MessageAllocatorT = typename MessageAllocTraits::allocator_type; @@ -384,9 +384,9 @@ class IntraProcessManager take_shared_subscriptions.size() <= 1) { // There is at maximum 1 buffer that does not require ownership. - // So we this case is equivalent to all the buffers requiring ownership + // So this case is equivalent to all the buffers requiring ownership - // Merge the two vector of ids into a unique one + // Merge the two vectors of ids into a unique one std::vector concatenated_vector(take_shared_subscriptions); concatenated_vector.insert( concatenated_vector.end(), @@ -418,6 +418,7 @@ class IntraProcessManager const std::vector & take_ownership_subscriptions, const std::vector & take_shared_subscriptions) { + // subsriber and publisher have different message types // get first subscription const auto subscription_id = take_ownership_subscriptions.empty() ? @@ -474,6 +475,7 @@ class IntraProcessManager const std::vector & take_ownership_subscriptions, const std::vector & take_shared_subscriptions) { + // subsriber and publisher have same message types using MessageAllocTraits = allocator::AllocRebind; using MessageAllocatorT = typename MessageAllocTraits::allocator_type; @@ -563,7 +565,7 @@ class IntraProcessManager } } - /// method for unknown allocator using subscription for allocation + /// Method for unknown allocator using subscription for allocation void add_owned_msg_to_buffers( std::shared_ptr message, @@ -591,11 +593,9 @@ class IntraProcessManager /// Convert received message to message type of subscriber /** - * Method for ros message for serialization to rclcpp::SerializedMessage. + * Method to serialize a ros message to rclcpp::SerializedMessage. * The publisher has a template type while the subscribers is serialized. * - * In addition this generates a unique intra process id for the publisher. - * * \param message the ros message from publisher. * \param subscription a subscriber used for creating the serialized message. * \return a shared pointer (containing rclcpp::SerializedMessage) with deleter. @@ -623,8 +623,6 @@ class IntraProcessManager * Overloaded method for rclcpp::SerializedMessage for deserialization. * The publisher is serialized while the subscribers have a templated message type. * - * In addition this generates a unique intra process id for the publisher. - * * \param serialized_message the serialized message from publisher. * \param subscription a subscriber used for creating ros message and serialization. * \return a shared pointer (containing a ros message) with deleter. From fd7c268a3011b525f83536543f640df503b46fe1 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 22 Apr 2020 17:02:02 -0300 Subject: [PATCH 017/121] Generate node interfaces' getters and traits. (#1069) Signed-off-by: Michel Hidalgo Co-authored-by: Karsten Knese --- rclcpp/CMakeLists.txt | 60 ++++++- rclcpp/include/rclcpp/create_timer.hpp | 4 +- .../get_node_base_interface.hpp | 149 ------------------ .../get_node_timers_interface.hpp | 149 ------------------ .../get_node_topics_interface.hpp | 149 ------------------ rclcpp/resource/get_interface.hpp.em | 111 +++++++++++++ rclcpp/resource/interface_traits.hpp.em | 47 ++++++ .../test_get_node_interfaces.cpp | 60 +++++-- rclcpp/test/test_interface_traits.cpp | 77 +++++++++ 9 files changed, 340 insertions(+), 466 deletions(-) delete mode 100644 rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp delete mode 100644 rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp delete mode 100644 rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp create mode 100644 rclcpp/resource/get_interface.hpp.em create mode 100644 rclcpp/resource/interface_traits.hpp.em create mode 100644 rclcpp/test/test_interface_traits.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 3918120e5a..1e0581451b 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -101,19 +101,65 @@ configure_file( COPYONLY ) # generate header with logging macros -set(python_code +set(python_code_logging "import em" "em.invoke(['-o', 'include/rclcpp/logging.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/logging.hpp.em'])") -string(REPLACE ";" "$" python_code "${python_code}") +string(REPLACE ";" "$" python_code_logging "${python_code_logging}") add_custom_command(OUTPUT include/rclcpp/logging.hpp COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp" - COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code}" + COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_logging}" DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch" COMMENT "Expanding logging.hpp.em" VERBATIM ) list(APPEND ${PROJECT_NAME}_SRCS include/rclcpp/logging.hpp) + +file(GLOB interface_files "include/rclcpp/node_interfaces/node_*_interface.hpp") +foreach(interface_file ${interface_files}) + get_filename_component(interface_name ${interface_file} NAME_WE) + + # "watch" template for changes + configure_file( + "resource/interface_traits.hpp.em" + "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch" + COPYONLY + ) + set(python_${interface_name}_traits + "import em" + "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/${interface_name}_traits.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/interface_traits.hpp.em'])") + string(REPLACE ";" "$" python_${interface_name}_traits "${python_${interface_name}_traits}") + add_custom_command(OUTPUT include/rclcpp/node_interfaces/${interface_name}_traits.hpp + COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces" + COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_${interface_name}_traits}" + DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch" + COMMENT "Expanding interface_traits.hpp.em into ${interface_name}_traits.hpp" + VERBATIM + ) + list(APPEND ${PROJECT_NAME}_SRCS + include/rclcpp/node_interfaces/${interface_name}_traits.hpp) + + # "watch" template for changes + configure_file( + "resource/get_interface.hpp.em" + "get_${interface_name}.hpp.em.watch" + COPYONLY + ) + set(python_get_${interface_name} + "import em" + "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/get_${interface_name}.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/get_interface.hpp.em'])") + string(REPLACE ";" "$" python_get_${interface_name} "${python_get_${interface_name}}") + add_custom_command(OUTPUT include/rclcpp/node_interfaces/get_${interface_name}.hpp + COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces" + COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_get_${interface_name}}" + DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/get_${interface_name}.hpp.em.watch" + COMMENT "Expanding get_interface.hpp.em into get_${interface_file}.hpp" + VERBATIM + ) + list(APPEND ${PROJECT_NAME}_SRCS + include/rclcpp/node_interfaces/get_${interface_name}.hpp) +endforeach() + include_directories("${CMAKE_CURRENT_BINARY_DIR}/include") add_library(${PROJECT_NAME} @@ -532,6 +578,14 @@ if(BUILD_TESTING) target_link_libraries(test_init ${PROJECT_NAME}) endif() + ament_add_gtest(test_interface_traits test/test_interface_traits.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_interface_traits) + ament_target_dependencies(test_interface_traits + "rcl") + target_link_libraries(test_interface_traits ${PROJECT_NAME}) + endif() + ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_multi_threaded_executor) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 208474f345..c541ba4616 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -32,8 +32,8 @@ namespace rclcpp template typename rclcpp::TimerBase::SharedPtr create_timer( - node_interfaces::NodeBaseInterface * node_base, - node_interfaces::NodeTimersInterface * node_timers, + std::shared_ptr node_base, + std::shared_ptr node_timers, rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback, diff --git a/rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp deleted file mode 100644 index cfc5917689..0000000000 --- a/rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp +++ /dev/null @@ -1,149 +0,0 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_ -#define RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_ - -#include -#include -#include - -#include "rclcpp/node_interfaces/node_base_interface.hpp" - -/// This header provides the get_node_base_interface() template function. -/** - * This function is useful for getting the NodeBaseInterface pointer from - * various kinds of Node-like classes. - * - * It's able to get the NodeBaseInterface pointer so long as the class - * has a method called ``get_node_base_interface()`` which returns - * either a pointer (const or not) to a NodeBaseInterface or a - * std::shared_ptr to a NodeBaseInterface. - */ - -namespace rclcpp -{ -namespace node_interfaces -{ - -namespace detail -{ - -// This is a meta-programming checker for if a given Node-like object has a -// getter called get_node_base_interface() which returns various types, -// e.g. const pointer or a shared pointer. -template -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; - - template - static constexpr - std::false_type - check(...); - -public: - using type = decltype(check(nullptr)); - static constexpr bool value = type::value; -}; - -// 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) -{ - return pointer; -} - -// 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) -{ - return node_pointer->get_node_base_interface().get(); -} - -// 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) -{ - return node_pointer->get_node_base_interface(); -} - -// 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) -{ - return get_node_base_interface_from_pointer(node_shared_pointer->get()); -} - -} // namespace detail - -/// 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) -{ - // Forward pointers to detail implmentation directly. - return detail::get_node_base_interface_from_pointer(node_pointer); -} - -/// Get the NodeBaseInterface as a pointer from a "Node like" object. -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) -{ - // Forward references to detail implmentation as a pointer. - return detail::get_node_base_interface_from_pointer(&node_reference); -} - -} // namespace node_interfaces -} // namespace rclcpp - -#endif // RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp deleted file mode 100644 index 9c54f06e9c..0000000000 --- a/rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp +++ /dev/null @@ -1,149 +0,0 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_ -#define RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_ - -#include -#include -#include - -#include "rclcpp/node_interfaces/node_timers_interface.hpp" - -/// This header provides the get_node_timers_interface() template function. -/** - * This function is useful for getting the NodeTimersInterface pointer from - * various kinds of Node-like classes. - * - * It's able to get the NodeTimersInterface pointer so long as the class - * has a method called ``get_node_timers_interface()`` which returns - * either a pointer (const or not) to a NodeTimersInterface or a - * std::shared_ptr to a NodeTimersInterface. - */ - -namespace rclcpp -{ -namespace node_interfaces -{ - -namespace detail -{ - -// This is a meta-programming checker for if a given Node-like object has a -// getter called get_node_timers_interface() which returns various types, -// e.g. const pointer or a shared pointer. -template -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; - - template - static constexpr - std::false_type - check(...); - -public: - using type = decltype(check(nullptr)); - static constexpr bool value = type::value; -}; - -// 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) -{ - return pointer; -} - -// 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) -{ - return node_pointer->get_node_timers_interface().get(); -} - -// 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) -{ - return node_pointer->get_node_timers_interface(); -} - -// 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) -{ - return get_node_timers_interface_from_pointer(node_shared_pointer->get()); -} - -} // namespace detail - -/// 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) -{ - // Forward pointers to detail implmentation directly. - return detail::get_node_timers_interface_from_pointer(node_pointer); -} - -/// Get the NodeTimersInterface as a pointer from a "Node like" object. -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) -{ - // Forward references to detail implmentation as a pointer. - return detail::get_node_timers_interface_from_pointer(&node_reference); -} - -} // namespace node_interfaces -} // namespace rclcpp - -#endif // RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp deleted file mode 100644 index c84234282f..0000000000 --- a/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp +++ /dev/null @@ -1,149 +0,0 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_ -#define RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_ - -#include -#include -#include - -#include "rclcpp/node_interfaces/node_topics_interface.hpp" - -/// This header provides the get_node_topics_interface() template function. -/** - * This function is useful for getting the NodeTopicsInterface pointer from - * various kinds of Node-like classes. - * - * It's able to get the NodeTopicsInterface pointer so long as the class - * has a method called ``get_node_topics_interface()`` which returns - * either a pointer (const or not) to a NodeTopicsInterface or a - * std::shared_ptr to a NodeTopicsInterface. - */ - -namespace rclcpp -{ -namespace node_interfaces -{ - -namespace detail -{ - -// This is a meta-programming checker for if a given Node-like object has a -// getter called get_node_topics_interface() which returns various types, -// e.g. const pointer or a shared pointer. -template -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; - - template - static constexpr - std::false_type - check(...); - -public: - using type = decltype(check(nullptr)); - static constexpr bool value = type::value; -}; - -// 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) -{ - return pointer; -} - -// 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) -{ - return node_pointer->get_node_topics_interface().get(); -} - -// 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) -{ - return node_pointer->get_node_topics_interface(); -} - -// 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) -{ - return get_node_topics_interface_from_pointer(node_shared_pointer->get()); -} - -} // namespace detail - -/// 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) -{ - // Forward pointers to detail implmentation directly. - return detail::get_node_topics_interface_from_pointer(node_pointer); -} - -/// Get the NodeTopicsInterface as a pointer from a "Node like" object. -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) -{ - // Forward references to detail implmentation as a pointer. - return detail::get_node_topics_interface_from_pointer(&node_reference); -} - -} // namespace node_interfaces -} // namespace rclcpp - -#endif // RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_ diff --git a/rclcpp/resource/get_interface.hpp.em b/rclcpp/resource/get_interface.hpp.em new file mode 100644 index 0000000000..879b405f7f --- /dev/null +++ b/rclcpp/resource/get_interface.hpp.em @@ -0,0 +1,111 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +@{ +uppercase_interface_name = interface_name.upper() +}@ + +#ifndef RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_ +#define RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_ + +#include +#include +#include + +#include "rcpputils/pointer_traits.hpp" + +#include "rclcpp/node_interfaces/@(interface_name).hpp" +#include "rclcpp/node_interfaces/@(interface_name)_traits.hpp" + +@{ +interface_typename = ''.join([part.capitalize() for part in interface_name.split('_')]) +}@ + +/// This header provides the get_@(interface_name)() template function. +/** + * This function is useful for getting the @(interface_typename) pointer from + * various kinds of Node-like classes. + * + * It's able to get a std::shared_ptr to a @(interface_typename) so long as the class + * has a method called ``get_@(interface_name)()`` which returns one. + */ + +namespace rclcpp +{ +namespace node_interfaces +{ +namespace detail +{ + +// If NodeType has a method called get_@(interface_name)() which returns a shared pointer. +template< + typename NodeType, + typename std::enable_if::type + >::value, int>::type = 0 +> +std::shared_ptr +get_@(interface_name)_from_pointer(NodeType node_pointer) +{ + if (!node_pointer) { + throw std::invalid_argument("node cannot be nullptr"); + } + return node_pointer->get_@(interface_name)(); +} + +} // namespace detail + +/// Get the @(interface_typename) as a shared pointer from a pointer to a "Node like" object. +template< + typename NodeType, + typename std::enable_if< + rcpputils::is_pointer::value, int + >::type = 0 +> +inline +std::shared_ptr +get_@(interface_name)(NodeType && node) +{ + // Forward pointers to detail implementation directly. + return detail::get_@(interface_name)_from_pointer(node); +} + +/// Get the @(interface_typename) as a shared pointer from a "Node like" object. +template< + typename NodeType, + typename std::enable_if< + !rcpputils::is_pointer::value, int + >::type = 0 +> +inline +std::shared_ptr +get_@(interface_name)(NodeType && node) +{ + // Forward references to detail implementation as a pointer. + return detail::get_@(interface_name)_from_pointer(&node); +} + +/// Keep the @(interface_typename) a shared pointer. +inline +std::shared_ptr +get_@(interface_name)( + std::shared_ptr & node_interface) +{ + return node_interface; +} + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_ diff --git a/rclcpp/resource/interface_traits.hpp.em b/rclcpp/resource/interface_traits.hpp.em new file mode 100644 index 0000000000..506d67fefd --- /dev/null +++ b/rclcpp/resource/interface_traits.hpp.em @@ -0,0 +1,47 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +@{ +uppercase_interface_name = interface_name.upper() +interface_typename = ''.join([part.capitalize() for part in interface_name.split('_')]) +}@ + +#ifndef RCLCPP__NODE_INTERFACES__@(uppercase_interface_name)_TRAITS_HPP_ +#define RCLCPP__NODE_INTERFACES__@(uppercase_interface_name)_TRAITS_HPP_ + +#include +#include + +#include "rclcpp/node_interfaces/@(interface_name).hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +template +struct has_@(interface_name) : std::false_type +{}; + +template +struct has_@(interface_name)< + T, typename std::enable_if< + std::is_same< + std::shared_ptr, + decltype(std::declval().get_@(interface_name)())>::value>::type> : std::true_type +{}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__@(uppercase_interface_name)_TRAITS_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..d91570e3be 100644 --- a/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp +++ b/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp @@ -47,22 +47,38 @@ class TestGetNodeInterfaces : public ::testing::Test rclcpp::Node::SharedPtr TestGetNodeInterfaces::node = nullptr; std::shared_ptr TestGetNodeInterfaces::wrapped_node = nullptr; +TEST_F(TestGetNodeInterfaces, null_rclcpp_node_shared_ptr) { + rclcpp::Node::SharedPtr null_node; + EXPECT_THROW( + { + rclcpp::node_interfaces::get_node_topics_interface(null_node); + }, std::invalid_argument); +} + 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 *, + std::shared_ptr, decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + >::value, "expected std::shared_ptr"); +} + +TEST_F(TestGetNodeInterfaces, null_node_shared_ptr) { + std::shared_ptr null_node; + EXPECT_THROW( + { + rclcpp::node_interfaces::get_node_topics_interface(null_node); + }, std::invalid_argument); } 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 *, + std::shared_ptr, decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + >::value, "expected std::shared_ptr"); } TEST_F(TestGetNodeInterfaces, rclcpp_node_reference) { @@ -70,9 +86,9 @@ TEST_F(TestGetNodeInterfaces, rclcpp_node_reference) { auto result = rclcpp::node_interfaces::get_node_topics_interface(node_reference); static_assert( std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, + std::shared_ptr, decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + >::value, "expected std::shared_ptr"); } TEST_F(TestGetNodeInterfaces, node_reference) { @@ -80,9 +96,9 @@ TEST_F(TestGetNodeInterfaces, node_reference) { auto result = rclcpp::node_interfaces::get_node_topics_interface(wrapped_node_reference); static_assert( std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, + std::shared_ptr, decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + >::value, "expected std::shared_ptr"); } TEST_F(TestGetNodeInterfaces, rclcpp_node_pointer) { @@ -90,9 +106,17 @@ TEST_F(TestGetNodeInterfaces, rclcpp_node_pointer) { auto result = rclcpp::node_interfaces::get_node_topics_interface(node_pointer); static_assert( std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, + std::shared_ptr, decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + >::value, "expected std::shared_ptr"); +} + +TEST_F(TestGetNodeInterfaces, null_rclcpp_node_pointer) { + rclcpp::Node * null_node{nullptr}; + EXPECT_THROW( + { + rclcpp::node_interfaces::get_node_topics_interface(null_node); + }, std::invalid_argument); } TEST_F(TestGetNodeInterfaces, node_pointer) { @@ -100,9 +124,17 @@ TEST_F(TestGetNodeInterfaces, node_pointer) { auto result = rclcpp::node_interfaces::get_node_topics_interface(wrapped_node_pointer); static_assert( std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, + std::shared_ptr, decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + >::value, "expected std::shared_ptr"); +} + +TEST_F(TestGetNodeInterfaces, null_node_pointer) { + NodeWrapper * null_node{nullptr}; + EXPECT_THROW( + { + rclcpp::node_interfaces::get_node_topics_interface(null_node); + }, std::invalid_argument); } TEST_F(TestGetNodeInterfaces, interface_shared_pointer) { @@ -111,7 +143,7 @@ TEST_F(TestGetNodeInterfaces, interface_shared_pointer) { auto result = rclcpp::node_interfaces::get_node_topics_interface(interface_shared_ptr); static_assert( std::is_same< - rclcpp::node_interfaces::NodeTopicsInterface *, + std::shared_ptr, decltype(result) - >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); + >::value, "expected std::shared_ptr"); } diff --git a/rclcpp/test/test_interface_traits.cpp b/rclcpp/test/test_interface_traits.cpp new file mode 100644 index 0000000000..56754b6c98 --- /dev/null +++ b/rclcpp/test/test_interface_traits.cpp @@ -0,0 +1,77 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/node_interfaces/node_base_interface_traits.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/node.hpp" + +class MyNode +{ +public: + std::shared_ptr get_node_base_interface() const + { + rclcpp::NodeOptions options; + return std::make_shared( + "my_node_name", + "my_node_namespace", + rclcpp::contexts::default_context::get_global_default_context(), + *options.get_rcl_node_options(), + false); + } +}; + +class WrongNode +{ +public: + std::shared_ptr not_get_node_base_interface() + { + return nullptr; + } +}; + +template::value + >::type * = nullptr> +void get_node_name(const T & nodelike) +{ + ASSERT_STREQ("my_node_name", nodelike.get_node_base_interface()->get_name()); +} + +class TestInterfaceTraits : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestInterfaceTraits, has_node_base_interface) { + ASSERT_TRUE(rclcpp::node_interfaces::has_node_base_interface::value); + ASSERT_FALSE(rclcpp::node_interfaces::has_node_base_interface::value); + ASSERT_TRUE(rclcpp::node_interfaces::has_node_base_interface::value); + + get_node_name(MyNode()); +} From de69944ddede41f6972348a0e2e83f1f8f8e22b4 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 22 Apr 2020 14:19:39 -0700 Subject: [PATCH 018/121] Fix rclcpp interface traits test (#1086) Signed-off-by: Karsten Knese --- rclcpp/test/test_interface_traits.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/test_interface_traits.cpp b/rclcpp/test/test_interface_traits.cpp index 56754b6c98..599517c328 100644 --- a/rclcpp/test/test_interface_traits.cpp +++ b/rclcpp/test/test_interface_traits.cpp @@ -33,7 +33,8 @@ class MyNode "my_node_namespace", rclcpp::contexts::default_context::get_global_default_context(), *options.get_rcl_node_options(), - false); + options.use_intra_process_comms(), + options.enable_topic_statistics()); } }; From e9d19a085497f7cf46adade976e98b618c38110f Mon Sep 17 00:00:00 2001 From: Devin Bonnie <47613035+dabonnie@users.noreply.github.com> Date: Wed, 22 Apr 2020 16:09:41 -0700 Subject: [PATCH 019/121] Integrate topic statistics (#1072) * Add SubscriberTopicStatistics class Signed-off-by: Devin Bonnie * Add SubscriberTopicStatistics Test Signed-off-by: Devin Bonnie * Address review comments Signed-off-by: Devin Bonnie * Modify constructor to allow a node to create necessary components Signed-off-by: Devin Bonnie * Fix docstring style Signed-off-by: Devin Bonnie * Remove SetPublisherTimer method Signed-off-by: Devin Bonnie * Change naming style to match rclcpp Signed-off-by: Devin Bonnie * Address style issues Signed-off-by: Devin Bonnie * Use rclcpp:Time Signed-off-by: Devin Bonnie * Address review comments Signed-off-by: Devin Bonnie * Remove unnecessary check for null publisher timer Move anonymous namespace function to private class method Signed-off-by: Devin Bonnie * Update message dependency Signed-off-by: Devin Bonnie * Initial integration of Subscriber Topic Statistics Signed-off-by: Devin Bonnie * Fix nanoseconds used for Topic Stats Signed-off-by: Devin Bonnie * Add simple publishing test Minor fixes Signed-off-by: Devin Bonnie * Add test utils header Signed-off-by: Devin Bonnie * Integrate with Topic Statistics options Fixes after rebasing with master Signed-off-by: Devin Bonnie * Update after rebasing Signed-off-by: Devin Bonnie * Address minor review comments Signed-off-by: Devin Bonnie * Move Topic Statistics instantiation to create_subscription Signed-off-by: Devin Bonnie * Fix rebase issue Fix topic statistics enable flag usage Address minor formatting Signed-off-by: Devin Bonnie * Move new timer creation method to relevant header Signed-off-by: Devin Bonnie * Add timers interface to topic interface Signed-off-by: Devin Bonnie * Use new create timer method Signed-off-by: Devin Bonnie * Address review comments Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/create_subscription.hpp | 50 +++++- rclcpp/include/rclcpp/create_timer.hpp | 42 +++++ .../resolve_enable_topic_statistics.hpp | 1 - rclcpp/include/rclcpp/node_impl.hpp | 13 +- .../rclcpp/node_interfaces/node_topics.hpp | 10 +- .../node_interfaces/node_topics_interface.hpp | 6 + rclcpp/include/rclcpp/subscription.hpp | 21 ++- .../include/rclcpp/subscription_factory.hpp | 11 +- rclcpp/src/rclcpp/node.cpp | 2 +- .../rclcpp/node_interfaces/node_topics.cpp | 12 +- .../test_subscription_topic_statistics.cpp | 125 +++++++++++++-- .../test_topic_stats_utils.hpp | 151 ++++++++++++++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 2 +- 13 files changed, 419 insertions(+), 27 deletions(-) create mode 100644 rclcpp/test/topic_statistics/test_topic_stats_utils.hpp diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 9248254047..207bee4a71 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -15,15 +15,25 @@ #ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_ #define RCLCPP__CREATE_SUBSCRIPTION_HPP_ +#include +#include #include #include #include +#include "rclcpp/detail/resolve_enable_topic_statistics.hpp" + +#include "rclcpp/node_interfaces/get_node_timers_interface.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" + +#include "rclcpp/create_timer.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/subscription_factory.hpp" #include "rclcpp/subscription_options.hpp" -#include "rclcpp/qos.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "rmw/qos_profiles.h" namespace rclcpp @@ -64,10 +74,46 @@ create_subscription( using rclcpp::node_interfaces::get_node_topics_interface; auto node_topics = get_node_topics_interface(std::forward(node)); + std::shared_ptr> + subscription_topic_stats = nullptr; + + if (rclcpp::detail::resolve_enable_topic_statistics( + options, + *node_topics->get_node_base_interface())) + { + std::shared_ptr> publisher = + create_publisher( + node, + options.topic_stats_options.publish_topic, + qos); + + subscription_topic_stats = std::make_shared< + rclcpp::topic_statistics::SubscriptionTopicStatistics + >(node_topics->get_node_base_interface()->get_name(), publisher); + + auto sub_call_back = [subscription_topic_stats]() { + subscription_topic_stats->publish_message(); + }; + + auto node_timer_interface = node_topics->get_node_timers_interface(); + + auto timer = create_wall_timer( + std::chrono::duration_cast( + options.topic_stats_options.publish_period), + sub_call_back, + options.callback_group, + node_topics->get_node_base_interface(), + node_timer_interface + ); + + subscription_topic_stats->set_publisher_timer(timer); + } + auto factory = rclcpp::create_subscription_factory( std::forward(callback), options, - msg_mem_strat + msg_mem_strat, + subscription_topic_stats ); auto sub = node_topics->create_subscription(topic_name, factory, qos); diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index c541ba4616..ed2965b01c 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__CREATE_TIMER_HPP_ #define RCLCPP__CREATE_TIMER_HPP_ +#include +#include #include #include #include @@ -68,6 +70,46 @@ create_timer( group); } +/// Convenience method to create a timer with node resources. +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \tparam CallbackT + * \param period period to exectute callback + * \param callback callback to execute via the timer period + * \param group + * \param node_base + * \param node_timers + * \return + * \throws std::invalid argument if either node_base or node_timers + * are null + */ +template +typename rclcpp::WallTimer::SharedPtr +create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group, + node_interfaces::NodeBaseInterface * node_base, + node_interfaces::NodeTimersInterface * node_timers) +{ + if (node_base == nullptr) { + throw std::invalid_argument{"input node_base cannot be null"}; + } + + if (node_timers == nullptr) { + throw std::invalid_argument{"input node_timers cannot be null"}; + } + + auto timer = rclcpp::WallTimer::make_shared( + std::chrono::duration_cast(period), + std::move(callback), + node_base->get_context()); + node_timers->add_timer(timer, group); + return timer; +} + } // namespace rclcpp #endif // RCLCPP__CREATE_TIMER_HPP_ diff --git a/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp b/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp index 7c59204ff5..e10a92ef10 100644 --- a/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp @@ -42,7 +42,6 @@ resolve_enable_topic_statistics(const OptionsT & options, const NodeBaseT & node break; default: throw std::runtime_error("Unrecognized EnableTopicStatistics value"); - break; } return topic_stats_enabled; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index e29edeb22e..512f472f79 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -37,10 +38,12 @@ #include "rclcpp/create_client.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" +#include "rclcpp/create_timer.hpp" #include "rclcpp/create_subscription.hpp" #include "rclcpp/detail/resolve_enable_topic_statistics.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/qos.hpp" +#include "rclcpp/timer.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -108,12 +111,12 @@ Node::create_wall_timer( CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - auto timer = rclcpp::WallTimer::make_shared( - std::chrono::duration_cast(period), + return rclcpp::create_wall_timer( + period, std::move(callback), - this->node_base_->get_context()); - node_timers_->add_timer(timer, group); - return timer; + group, + this->node_base_.get(), + this->node_timers_.get()); } template diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index 8209984abc..227d193516 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -22,6 +22,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/publisher_factory.hpp" @@ -39,7 +40,9 @@ class NodeTopics : public NodeTopicsInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface) RCLCPP_PUBLIC - explicit NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base); + NodeTopics( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeTimersInterface * node_timers); RCLCPP_PUBLIC ~NodeTopics() override; @@ -74,10 +77,15 @@ class NodeTopics : public NodeTopicsInterface rclcpp::node_interfaces::NodeBaseInterface * get_node_base_interface() const override; + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeTimersInterface * + get_node_timers_interface() const override; + private: RCLCPP_DISABLE_COPY(NodeTopics) rclcpp::node_interfaces::NodeBaseInterface * node_base_; + rclcpp::node_interfaces::NodeTimersInterface * node_timers_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index 857670c58b..4ba28d5e40 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -25,6 +25,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/publisher_factory.hpp" #include "rclcpp/subscription.hpp" @@ -80,6 +81,11 @@ class NodeTopicsInterface virtual rclcpp::node_interfaces::NodeBaseInterface * get_node_base_interface() const = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::node_interfaces::NodeTimersInterface * + get_node_timers_interface() const = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2b5172dc1b..a1e544c773 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -25,7 +26,6 @@ #include #include - #include "rcl/error_handling.h" #include "rcl/subscription.h" @@ -47,6 +47,7 @@ #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" #include "rclcpp/waitable.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "tracetools/tracetools.h" namespace rclcpp @@ -75,6 +76,8 @@ class Subscription : public SubscriptionBase using MessageDeleter = allocator::Deleter; using ConstMessageSharedPtr = std::shared_ptr; using MessageUniquePtr = std::unique_ptr; + using SubscriptionTopicStatisticsSharedPtr = + std::shared_ptr>; RCLCPP_SMART_PTR_DEFINITIONS(Subscription) @@ -98,7 +101,8 @@ class Subscription : public SubscriptionBase const rclcpp::QoS & qos, AnySubscriptionCallback callback, const rclcpp::SubscriptionOptionsWithAllocator & options, - typename MessageMemoryStrategyT::SharedPtr message_memory_strategy) + typename MessageMemoryStrategyT::SharedPtr message_memory_strategy, + SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics = nullptr) : SubscriptionBase( node_base, type_support_handle, @@ -180,6 +184,10 @@ class Subscription : public SubscriptionBase this->setup_intra_process(intra_process_subscription_id, ipm); } + if (subscription_topic_statistics != nullptr) { + this->subscription_topic_statistics_ = std::move(subscription_topic_statistics); + } + TRACEPOINT( rclcpp_subscription_init, (const void *)get_subscription_handle().get(), @@ -260,6 +268,13 @@ class Subscription : public SubscriptionBase } auto typed_message = std::static_pointer_cast(message); any_callback_.dispatch(typed_message, message_info); + + if (subscription_topic_statistics_) { + const auto nanos = std::chrono::time_point_cast( + std::chrono::steady_clock::now()); + const auto time = rclcpp::Time(nanos.time_since_epoch().count(), RCL_STEADY_TIME); + subscription_topic_statistics_->handle_message(*typed_message, time); + } } void @@ -307,6 +322,8 @@ class Subscription : public SubscriptionBase const rclcpp::SubscriptionOptionsWithAllocator options_; typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy_; + /// Component which computes and publishes topic statistics for this subscriber + SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index a0f265c803..ffab0724c8 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -32,6 +32,7 @@ #include "rclcpp/subscription_options.hpp" #include "rclcpp/subscription_traits.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" namespace rclcpp { @@ -78,7 +79,10 @@ SubscriptionFactory create_subscription_factory( CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options, - typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat, + std::shared_ptr> + subscription_topic_stats = nullptr +) { auto allocator = options.get_allocator(); @@ -88,7 +92,7 @@ create_subscription_factory( SubscriptionFactory factory { // factory function that creates a MessageT specific SubscriptionT - [options, msg_mem_strat, any_subscription_callback]( + [options, msg_mem_strat, any_subscription_callback, subscription_topic_stats]( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, const rclcpp::QoS & qos @@ -104,7 +108,8 @@ create_subscription_factory( qos, any_subscription_callback, options, - msg_mem_strat); + msg_mem_strat, + subscription_topic_stats); // This is used for setting up things like intra process comms which // require this->shared_from_this() which cannot be called from // the constructor. diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index b429b305c2..b73ec6bf2f 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -108,7 +108,7 @@ Node::Node( 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_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), node_clock_(new rclcpp::node_interfaces::NodeClock( node_base_, diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 11de4a3278..360b4236c3 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -22,8 +22,10 @@ using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::node_interfaces::NodeTopics; -NodeTopics::NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base) -: node_base_(node_base) +NodeTopics::NodeTopics( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeTimersInterface * node_timers) +: node_base_(node_base), node_timers_(node_timers) {} NodeTopics::~NodeTopics() @@ -121,3 +123,9 @@ NodeTopics::get_node_base_interface() const { return node_base_; } + +rclcpp::node_interfaces::NodeTimersInterface * +NodeTopics::get_node_timers_interface() const +{ + return node_timers_; +} diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index de90ecb590..416c7fc4ad 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -14,6 +14,9 @@ #include +#include +#include +#include #include #include #include @@ -24,23 +27,34 @@ #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/subscription_options.hpp" #include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "statistics_msgs/msg/metrics_message.hpp" +#include "statistics_msgs/msg/statistic_data_point.hpp" +#include "statistics_msgs/msg/statistic_data_type.hpp" + #include "test_msgs/msg/empty.hpp" +#include "test_topic_stats_utils.hpp" + namespace { -constexpr const char kTestNodeName[]{"test_sub_stats_node"}; +constexpr const char kTestPubNodeName[]{"test_pub_stats_node"}; +constexpr const char kTestSubNodeName[]{"test_sub_stats_node"}; constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"}; constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}; constexpr const uint64_t kNoSamples{0}; +constexpr const std::chrono::milliseconds kTestStatsPublishPeriod{5000}; +constexpr const std::chrono::seconds kTestDuration{10}; } // namespace using test_msgs::msg::Empty; -using statistics_msgs::msg::MetricsMessage; using rclcpp::topic_statistics::SubscriptionTopicStatistics; +using statistics_msgs::msg::MetricsMessage; +using statistics_msgs::msg::StatisticDataPoint; +using statistics_msgs::msg::StatisticDataType; using libstatistics_collector::moving_average_statistics::StatisticData; template @@ -63,6 +77,44 @@ class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics(topic, 10); + publish_timer_ = this->create_wall_timer( + publish_period, [this]() { + this->publish_message(); + }); + } + + virtual ~EmptyPublisher() = default; + + size_t get_number_published() + { + return number_published_.load(); + } + +private: + void publish_message() + { + ++number_published_; + auto msg = Empty{}; + publisher_->publish(msg); + } + + rclcpp::Publisher::SharedPtr publisher_; + std::atomic number_published_{0}; + rclcpp::TimerBase::SharedPtr publish_timer_; +}; + /** * Empty subscriber node: used to create subscriber topic statistics requirements */ @@ -72,6 +124,10 @@ class EmptySubscriber : public rclcpp::Node EmptySubscriber(const std::string & name, const std::string & topic) : Node(name) { + // manually enable topic statistics via options + auto options = rclcpp::SubscriptionOptions(); + options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; + auto callback = [this](Empty::UniquePtr msg) { this->receive_message(*msg); }; @@ -79,16 +135,15 @@ class EmptySubscriber : public rclcpp::Node std::function>( topic, rclcpp::QoS(rclcpp::KeepAll()), - callback); + callback, + options); } - virtual ~EmptySubscriber() = default; private: void receive_message(const Empty &) const { } - rclcpp::Subscription::SharedPtr subscription_; }; @@ -102,7 +157,7 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test { rclcpp::init(0 /* argc */, nullptr /* argv */); empty_subscriber = std::make_shared( - kTestNodeName, + kTestSubNodeName, kTestSubStatsTopic); } @@ -122,13 +177,11 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) kTestTopicStatisticsTopic, 10); - // construct the instance + // construct a separate instance auto sub_topic_stats = std::make_unique>( empty_subscriber->get_name(), topic_stats_publisher); - using libstatistics_collector::moving_average_statistics::StatisticData; - // expect no data has been collected / no samples received for (const auto & data : sub_topic_stats->get_current_collector_data()) { EXPECT_TRUE(std::isnan(data.average)); @@ -138,3 +191,57 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) EXPECT_EQ(kNoSamples, data.sample_count); } } + +TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_single_empty_stats_message) +{ + // create an empty publisher + auto empty_publisher = std::make_shared( + kTestPubNodeName, + kTestSubStatsTopic); + // empty_subscriber has a topic statistics instance as part of its subscription + // this will listen to and generate statistics for the empty message + + // create a listener for topic statistics messages + auto statistics_listener = std::make_shared( + "test_receive_single_empty_stats_message_listener", + "/statistics"); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(empty_publisher); + ex.add_node(statistics_listener); + ex.add_node(empty_subscriber); + + // spin and get future + ex.spin_until_future_complete( + statistics_listener->GetFuture(), + kTestDuration); + + // compare message counts, sample count should be the same as published and received count + EXPECT_EQ(1, statistics_listener->GetNumberOfMessagesReceived()); + + // check the received message and the data types + const auto received_message = statistics_listener->GetLastReceivedMessage(); + for (const auto & stats_point : received_message.statistics) { + const auto type = stats_point.data_type; + switch (type) { + case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: + EXPECT_LT(0, stats_point.data) << "unexpected sample count"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: + EXPECT_LT(0, stats_point.data) << "unexpected avg"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected min"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected max"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: + EXPECT_LT(0, stats_point.data) << "unexpected stddev"; + break; + default: + FAIL() << "received unknown statistics type: " << std::dec << + static_cast(type); + } + } +} diff --git a/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp b/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp new file mode 100644 index 0000000000..0140f919e8 --- /dev/null +++ b/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp @@ -0,0 +1,151 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "statistics_msgs/msg/metrics_message.hpp" + +#ifndef TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ +#define TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ + +namespace rclcpp +{ +namespace topic_statistics +{ + +using statistics_msgs::msg::MetricsMessage; + +/** +* Provide an interface to wait for a promise to be satisfied via its future. +*/ +class PromiseSetter +{ +public: + /** + * Reassign the promise member and return it's future. Acquires a mutex in order + * to mutate member variables. + * + * \return the promise member's future, called upon PeriodicMeasurement + */ + std::shared_future GetFuture() + { + std::unique_lock ulock{mutex_}; + use_future_ = true; + promise_ = std::promise(); + return promise_.get_future(); + } + +protected: + /** + * Set the promise to true, which signals the corresponding future. Acquires a mutex and sets + * the promise to true iff GetFuture was invoked before this. + */ + void SetPromise() + { + std::unique_lock ulock{mutex_}; + if (use_future_) { + // only set if GetFuture was called + promise_.set_value(true); + use_future_ = false; // the promise needs to be reassigned to set again + } + } + +private: + mutable std::mutex mutex_; + std::promise promise_; + bool use_future_{false}; +}; + +/** + * Node which listens for published MetricsMessages. This uses the PromiseSetter API + * in order to signal, via a future, that rclcpp should stop spinning upon + * message handling. + */ +class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter +{ +public: + /** + * Constructs a MetricsMessageSubscriber. + * \param name the node name + * \param name the topic name + * \param number of messages to receive to trigger the PromiseSetter future + */ + MetricsMessageSubscriber( + const std::string & name, + const std::string & topic_name, + const int number_of_messages_to_receive = 1) + : rclcpp::Node(name), + number_of_messages_to_receive_(number_of_messages_to_receive) + { + auto callback = [this](MetricsMessage::UniquePtr msg) { + this->MetricsMessageCallback(*msg); + }; + subscription_ = create_subscription>( + topic_name, + 0 /*history_depth*/, + callback); + } + + /** + * Acquires a mutex in order to get the last message received member. + * \return the last message received + */ + MetricsMessage GetLastReceivedMessage() const + { + std::unique_lock ulock{mutex_}; + return last_received_message_; + } + + /** + * Return the number of messages received by this subscriber. + * \return the number of messages received by the subscriber callback + */ + int GetNumberOfMessagesReceived() const + { + return num_messages_received_; + } + +private: + /** + * Subscriber callback. Acquires a mutex to set the last message received and + * sets the promise to true. + * \param msg + */ + void MetricsMessageCallback(const MetricsMessage & msg) + { + std::unique_lock ulock{mutex_}; + ++num_messages_received_; + last_received_message_ = msg; + if (num_messages_received_ >= number_of_messages_to_receive_) { + PromiseSetter::SetPromise(); + } + } + + MetricsMessage last_received_message_; + rclcpp::Subscription::SharedPtr subscription_; + mutable std::mutex mutex_; + std::atomic num_messages_received_{0}; + const int number_of_messages_to_receive_; +}; + +} // namespace topic_statistics +} // namespace rclcpp + +#endif // TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 0d6330b4cd..60debba6f7 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -67,7 +67,7 @@ LifecycleNode::LifecycleNode( 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_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), node_clock_(new rclcpp::node_interfaces::NodeClock( node_base_, From 801cb257d1af0183a84a66b96e4a78d2595f49d9 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 22 Apr 2020 19:30:56 -0700 Subject: [PATCH 020/121] Use serialized message (#1081) * use serialized message in callback Signed-off-by: Karsten Knese * introduce resize method for serialized message Signed-off-by: Karsten Knese * introduce release for serialized message Signed-off-by: Karsten Knese * address review comments Signed-off-by: Karsten Knese * correct typo Signed-off-by: Karsten Knese * fix interface traits test Signed-off-by: Karsten Knese --- .../rclcpp/message_memory_strategy.hpp | 32 ++++--------------- rclcpp/include/rclcpp/publisher.hpp | 6 ++++ rclcpp/include/rclcpp/serialized_message.hpp | 14 ++++++++ rclcpp/include/rclcpp/subscription.hpp | 4 +-- rclcpp/include/rclcpp/subscription_base.hpp | 7 ++-- rclcpp/src/rclcpp/executor.cpp | 3 +- rclcpp/src/rclcpp/serialized_message.cpp | 16 ++++++++++ rclcpp/src/rclcpp/subscription_base.cpp | 4 +-- rclcpp/test/test_serialized_message.cpp | 25 +++++++++++++++ .../test_serialized_message_allocator.cpp | 18 +++++------ rclcpp/test/test_subscription.cpp | 6 ++-- 11 files changed, 89 insertions(+), 46 deletions(-) diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index 80654d79e7..6e4c0be065 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -46,10 +46,10 @@ class MessageMemoryStrategy using MessageAlloc = typename MessageAllocTraits::allocator_type; using MessageDeleter = allocator::Deleter; - using SerializedMessageAllocTraits = allocator::AllocRebind; + using SerializedMessageAllocTraits = allocator::AllocRebind; using SerializedMessageAlloc = typename SerializedMessageAllocTraits::allocator_type; using SerializedMessageDeleter = - allocator::Deleter; + allocator::Deleter; using BufferAllocTraits = allocator::AllocRebind; using BufferAlloc = typename BufferAllocTraits::allocator_type; @@ -86,31 +86,12 @@ class MessageMemoryStrategy return std::allocate_shared(*message_allocator_.get()); } - virtual std::shared_ptr borrow_serialized_message(size_t capacity) + virtual std::shared_ptr borrow_serialized_message(size_t capacity) { - auto msg = new rcl_serialized_message_t; - *msg = rmw_get_zero_initialized_serialized_message(); - auto ret = rmw_serialized_message_init(msg, capacity, &rcutils_allocator_); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - - auto serialized_msg = std::shared_ptr( - msg, - [](rmw_serialized_message_t * msg) { - auto fini_ret = rmw_serialized_message_fini(msg); - delete msg; - if (fini_ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy serialized message: %s", rcl_get_error_string().str); - } - }); - - return serialized_msg; + return std::make_shared(capacity); } - virtual std::shared_ptr borrow_serialized_message() + virtual std::shared_ptr borrow_serialized_message() { return borrow_serialized_message(default_buffer_capacity_); } @@ -127,7 +108,8 @@ class MessageMemoryStrategy msg.reset(); } - virtual void return_serialized_message(std::shared_ptr & serialized_msg) + virtual void return_serialized_message( + std::shared_ptr & serialized_msg) { serialized_msg.reset(); } diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index aa614e3a0c..abe259b31d 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -217,6 +217,12 @@ class Publisher : public PublisherBase return this->do_serialized_publish(&serialized_msg); } + void + publish(const SerializedMessage & serialized_msg) + { + return this->do_serialized_publish(&serialized_msg.get_rcl_serialized_message()); + } + /// Publish an instance of a LoanedMessage. /** * When publishing a loaned message, the memory for this ROS message will be deallocated diff --git a/rclcpp/include/rclcpp/serialized_message.hpp b/rclcpp/include/rclcpp/serialized_message.hpp index 13c2d1910b..4b90bf3561 100644 --- a/rclcpp/include/rclcpp/serialized_message.hpp +++ b/rclcpp/include/rclcpp/serialized_message.hpp @@ -98,6 +98,20 @@ class RCLCPP_PUBLIC_TYPE SerializedMessage */ size_t capacity() const; + /// Allocate memory in the data buffer + /** + * The data buffer of the underlying rcl_serialized_message_t will be resized. + * This might change the data layout and invalidates all pointers to the data. + */ + void reserve(size_t capacity); + + /// Release the underlying rcl_serialized_message_t + /** + * The memory (i.e. the data buffer) of the serialized message will no longer + * be managed by this instance and the memory won't be deallocated on destruction. + */ + rcl_serialized_message_t release_rcl_serialized_message(); + private: rcl_serialized_message_t serialized_message_; }; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index a1e544c773..91343aaee0 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -250,7 +250,7 @@ class Subscription : public SubscriptionBase return message_memory_strategy_->borrow_message(); } - std::shared_ptr + std::shared_ptr create_serialized_message() override { return message_memory_strategy_->borrow_serialized_message(); @@ -299,7 +299,7 @@ class Subscription : public SubscriptionBase } void - return_serialized_message(std::shared_ptr & message) override + return_serialized_message(std::shared_ptr & message) override { message_memory_strategy_->return_serialized_message(message); } diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 7ee8c624d1..8156622299 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -33,6 +33,7 @@ #include "rclcpp/message_info.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" +#include "rclcpp/serialized_message.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -151,7 +152,7 @@ class SubscriptionBase : public std::enable_shared_from_this */ RCLCPP_PUBLIC bool - take_serialized(rcl_serialized_message_t & message_out, rclcpp::MessageInfo & message_info_out); + take_serialized(rclcpp::SerializedMessage & message_out, rclcpp::MessageInfo & message_info_out); /// Borrow a new message. /** \return Shared pointer to the fresh message. */ @@ -164,7 +165,7 @@ class SubscriptionBase : public std::enable_shared_from_this /** \return Shared pointer to a rcl_message_serialized_t. */ RCLCPP_PUBLIC virtual - std::shared_ptr + std::shared_ptr create_serialized_message() = 0; /// Check if we need to handle the message, and execute the callback if we do. @@ -194,7 +195,7 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_PUBLIC virtual void - return_serialized_message(std::shared_ptr & message) = 0; + return_serialized_message(std::shared_ptr & message) = 0; RCLCPP_PUBLIC const rosidl_message_type_support_t & diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 556c42ddde..c25b47db49 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -350,8 +350,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) if (subscription->is_serialized()) { // This is the case where a copy of the serialized message is taken from // the middleware via inter-process communication. - std::shared_ptr serialized_msg = - subscription->create_serialized_message(); + std::shared_ptr serialized_msg = subscription->create_serialized_message(); take_and_do_error_handling( "taking a serialized message from topic", subscription->get_topic_name(), diff --git a/rclcpp/src/rclcpp/serialized_message.cpp b/rclcpp/src/rclcpp/serialized_message.cpp index a667977cea..64e54f58f9 100644 --- a/rclcpp/src/rclcpp/serialized_message.cpp +++ b/rclcpp/src/rclcpp/serialized_message.cpp @@ -144,4 +144,20 @@ size_t SerializedMessage::capacity() const { return serialized_message_.buffer_capacity; } + +void SerializedMessage::reserve(size_t capacity) +{ + auto ret = rmw_serialized_message_resize(&serialized_message_, capacity); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +rcl_serialized_message_t SerializedMessage::release_rcl_serialized_message() +{ + auto ret = serialized_message_; + serialized_message_ = rmw_get_zero_initialized_serialized_message(); + + return ret; +} } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 2af068e32e..de1acfadb2 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -159,12 +159,12 @@ SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & mes bool SubscriptionBase::take_serialized( - rcl_serialized_message_t & message_out, + rclcpp::SerializedMessage & message_out, rclcpp::MessageInfo & message_info_out) { rcl_ret_t ret = rcl_take_serialized_message( this->get_subscription_handle().get(), - &message_out, + &message_out.get_rcl_serialized_message(), &message_info_out.get_rmw_message_info(), nullptr); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { diff --git a/rclcpp/test/test_serialized_message.cpp b/rclcpp/test/test_serialized_message.cpp index 0c8a037490..9047e22b22 100644 --- a/rclcpp/test/test_serialized_message.cpp +++ b/rclcpp/test/test_serialized_message.cpp @@ -114,6 +114,31 @@ TEST(TestSerializedMessage, various_constructors_from_rcl) { EXPECT_TRUE(nullptr != rcl_handle.buffer); } +TEST(TestSerializedMessage, release) { + const std::string content = "Hello World"; + const auto content_size = content.size() + 1; // accounting for null terminator + + rcl_serialized_message_t released_handle = rmw_get_zero_initialized_serialized_message(); + { + rclcpp::SerializedMessage serialized_msg(13); + // manually copy some content + auto & rcl_serialized_msg = serialized_msg.get_rcl_serialized_message(); + std::memcpy(rcl_serialized_msg.buffer, content.c_str(), content.size()); + rcl_serialized_msg.buffer[content.size()] = '\0'; + rcl_serialized_msg.buffer_length = content_size; + EXPECT_EQ(13u, serialized_msg.capacity()); + + released_handle = serialized_msg.release_rcl_serialized_message(); + // scope exit of serialized_msg + } + + EXPECT_TRUE(nullptr != released_handle.buffer); + EXPECT_EQ(13u, released_handle.buffer_capacity); + EXPECT_EQ(content_size, released_handle.buffer_length); + // cleanup memory manually + EXPECT_EQ(RCL_RET_OK, rmw_serialized_message_fini(&released_handle)); +} + TEST(TestSerializedMessage, serialization) { using MessageT = test_msgs::msg::BasicTypes; diff --git a/rclcpp/test/test_serialized_message_allocator.cpp b/rclcpp/test/test_serialized_message_allocator.cpp index bc177722e4..968ad0ac2d 100644 --- a/rclcpp/test/test_serialized_message_allocator.cpp +++ b/rclcpp/test/test_serialized_message_allocator.cpp @@ -29,25 +29,25 @@ TEST(TestSerializedMessageAllocator, default_allocator) { rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); auto msg0 = mem_strategy->borrow_serialized_message(); - ASSERT_EQ(msg0->buffer_capacity, 0u); + ASSERT_EQ(msg0->capacity(), 0u); mem_strategy->return_serialized_message(msg0); auto msg100 = mem_strategy->borrow_serialized_message(100); - ASSERT_EQ(msg100->buffer_capacity, 100u); + ASSERT_EQ(msg100->capacity(), 100u); mem_strategy->return_serialized_message(msg100); auto msg200 = mem_strategy->borrow_serialized_message(); - auto ret = rmw_serialized_message_resize(msg200.get(), 200); + auto ret = rmw_serialized_message_resize(&msg200->get_rcl_serialized_message(), 200); ASSERT_EQ(RCL_RET_OK, ret); - EXPECT_EQ(0u, msg200->buffer_length); - EXPECT_EQ(200u, msg200->buffer_capacity); + EXPECT_EQ(0u, msg200->size()); + EXPECT_EQ(200u, msg200->capacity()); mem_strategy->return_serialized_message(msg200); auto msg1000 = mem_strategy->borrow_serialized_message(1000); - ASSERT_EQ(msg1000->buffer_capacity, 1000u); - ret = rmw_serialized_message_resize(msg1000.get(), 2000); + ASSERT_EQ(msg1000->capacity(), 1000u); + ret = rmw_serialized_message_resize(&msg1000->get_rcl_serialized_message(), 2000); ASSERT_EQ(RCL_RET_OK, ret); - EXPECT_EQ(2000u, msg1000->buffer_capacity); + EXPECT_EQ(2000u, msg1000->capacity()); mem_strategy->return_serialized_message(msg1000); } @@ -61,7 +61,7 @@ TEST(TestSerializedMessageAllocator, borrow_from_subscription) { [](std::shared_ptr test_msg) {(void) test_msg;}); auto msg0 = sub->create_serialized_message(); - EXPECT_EQ(0u, msg0->buffer_capacity); + EXPECT_EQ(0u, msg0->capacity()); sub->return_serialized_message(msg0); rclcpp::shutdown(); diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp index 95f9b96448..a265f05e17 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -299,10 +299,10 @@ TEST_F(TestSubscription, take) { TEST_F(TestSubscription, take_serialized) { initialize(); using test_msgs::msg::Empty; - auto do_nothing = [](std::shared_ptr) {FAIL();}; + auto do_nothing = [](std::shared_ptr) {FAIL();}; { auto sub = node->create_subscription("~/test_take", 1, do_nothing); - std::shared_ptr msg = sub->create_serialized_message(); + std::shared_ptr msg = sub->create_serialized_message(); rclcpp::MessageInfo msg_info; EXPECT_FALSE(sub->take_serialized(*msg, msg_info)); } @@ -317,7 +317,7 @@ TEST_F(TestSubscription, take_serialized) { test_msgs::msg::Empty msg; pub->publish(msg); } - std::shared_ptr msg = sub->create_serialized_message(); + std::shared_ptr msg = sub->create_serialized_message(); rclcpp::MessageInfo msg_info; bool message_recieved = false; auto start = std::chrono::steady_clock::now(); From 90626d14537f1d4bf11c8774bec5ac3c227fa159 Mon Sep 17 00:00:00 2001 From: Devin Bonnie <47613035+dabonnie@users.noreply.github.com> Date: Wed, 22 Apr 2020 21:45:50 -0700 Subject: [PATCH 021/121] Remove unused test variable (#1087) Signed-off-by: Devin Bonnie --- .../test/topic_statistics/test_subscription_topic_statistics.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 416c7fc4ad..37433f0f2f 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -46,7 +46,6 @@ constexpr const char kTestSubNodeName[]{"test_sub_stats_node"}; constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"}; constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}; constexpr const uint64_t kNoSamples{0}; -constexpr const std::chrono::milliseconds kTestStatsPublishPeriod{5000}; constexpr const std::chrono::seconds kTestDuration{10}; } // namespace From 7bf74f9fc16a0f007d88e68f08494ced8a69ae31 Mon Sep 17 00:00:00 2001 From: tomoya Date: Thu, 23 Apr 2020 15:55:04 +0900 Subject: [PATCH 022/121] protect subscriber_statistics_collectors_ with a mutex (#1084) * subscriber_statistics_collectors_ should be protected with mutex. Co-Authored-By: William Woodall Signed-off-by: Tomoya.Fujita --- .../subscription_topic_statistics.hpp | 63 ++++++++++++++----- 1 file changed, 47 insertions(+), 16 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index 711b04998c..058ed45818 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -94,6 +94,8 @@ class SubscriptionTopicStatistics /// Handle a message received by the subscription to collect statistics. /** + * This method acquires a lock to prevent race conditions to collectors list. + * * \param received_message the message received by the subscription * \param now_nanoseconds current time in nanoseconds */ @@ -101,6 +103,7 @@ class SubscriptionTopicStatistics const CallbackMessageT & received_message, const rclcpp::Time now_nanoseconds) const { + std::lock_guard lock(mutex_); for (const auto & collector : subscriber_statistics_collectors_) { collector->OnMessageReceived(received_message, now_nanoseconds.nanoseconds()); } @@ -116,21 +119,32 @@ class SubscriptionTopicStatistics } /// Publish a populated MetricsStatisticsMessage. + /** + * This method acquires a lock to prevent race conditions to collectors list. + */ virtual void publish_message() { + std::vector msgs; rclcpp::Time window_end{get_current_nanoseconds_since_epoch()}; - for (auto & collector : subscriber_statistics_collectors_) { - const auto collected_stats = collector->GetStatisticsResults(); - - auto message = libstatistics_collector::collector::GenerateStatisticMessage( - node_name_, - collector->GetMetricName(), - collector->GetMetricUnit(), - window_start_, - window_end, - collected_stats); - publisher_->publish(message); + { + std::lock_guard lock(mutex_); + for (auto & collector : subscriber_statistics_collectors_) { + const auto collected_stats = collector->GetStatisticsResults(); + + auto message = libstatistics_collector::collector::GenerateStatisticMessage( + node_name_, + collector->GetMetricName(), + collector->GetMetricUnit(), + window_start_, + window_end, + collected_stats); + msgs.push_back(message); + } + } + + for (auto & msg : msgs) { + publisher_->publish(msg); } window_start_ = window_end; } @@ -138,11 +152,14 @@ class SubscriptionTopicStatistics protected: /// Return a vector of all the currently collected data. /** + * This method acquires a lock to prevent race conditions to collectors list. + * * \return a vector of all the collected data */ std::vector get_current_collector_data() const { std::vector data; + std::lock_guard lock(mutex_); for (const auto & collector : subscriber_statistics_collectors_) { data.push_back(collector->GetStatisticsResults()); } @@ -151,23 +168,35 @@ class SubscriptionTopicStatistics private: /// Construct and start all collectors and set window_start_. + /** + * This method acquires a lock to prevent race conditions to collectors list. + */ void bring_up() { auto received_message_period = std::make_unique(); received_message_period->Start(); - subscriber_statistics_collectors_.emplace_back(std::move(received_message_period)); + { + std::lock_guard lock(mutex_); + subscriber_statistics_collectors_.emplace_back(std::move(received_message_period)); + } window_start_ = rclcpp::Time(get_current_nanoseconds_since_epoch()); } /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher. + /** + * This method acquires a lock to prevent race conditions to collectors list. + */ void tear_down() { - for (auto & collector : subscriber_statistics_collectors_) { - collector->Stop(); - } + { + std::lock_guard lock(mutex_); + for (auto & collector : subscriber_statistics_collectors_) { + collector->Stop(); + } - subscriber_statistics_collectors_.clear(); + subscriber_statistics_collectors_.clear(); + } if (publisher_timer_) { publisher_timer_->cancel(); @@ -187,6 +216,8 @@ class SubscriptionTopicStatistics return std::chrono::duration_cast(now.time_since_epoch()).count(); } + /// Mutex to protect the subsequence vectors + mutable std::mutex mutex_; /// Collection of statistics collectors std::vector> subscriber_statistics_collectors_{}; /// Node name used to generate topic statistics messages to be published From 564e0e47c8815d507370ab8aad3290ea84b38f94 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 23 Apr 2020 11:09:40 -0700 Subject: [PATCH 023/121] adapt subscription traits to rclcpp::SerializedMessage (#1092) Signed-off-by: Karsten Knese --- rclcpp/include/rclcpp/serialization.hpp | 4 ---- rclcpp/include/rclcpp/subscription_traits.hpp | 9 --------- rclcpp/test/test_subscription_traits.cpp | 12 ++++++------ 3 files changed, 6 insertions(+), 19 deletions(-) diff --git a/rclcpp/include/rclcpp/serialization.hpp b/rclcpp/include/rclcpp/serialization.hpp index bc9063a9b8..787d18c523 100644 --- a/rclcpp/include/rclcpp/serialization.hpp +++ b/rclcpp/include/rclcpp/serialization.hpp @@ -39,10 +39,6 @@ template struct is_serialized_message_class : std::false_type {}; -template<> -struct is_serialized_message_class: std::true_type -{}; - template<> struct is_serialized_message_class: std::true_type {}; diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp index 3a84764a75..8eacd3a14b 100644 --- a/rclcpp/include/rclcpp/subscription_traits.hpp +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -42,15 +42,6 @@ template struct is_serialized_subscription_argument : std::false_type {}; -template<> -struct is_serialized_subscription_argument: std::true_type -{}; - -template<> -struct is_serialized_subscription_argument> - : std::true_type -{}; - template<> struct is_serialized_subscription_argument: std::true_type {}; diff --git a/rclcpp/test/test_subscription_traits.cpp b/rclcpp/test/test_subscription_traits.cpp index c99f25bb97..c7b062aefc 100644 --- a/rclcpp/test/test_subscription_traits.cpp +++ b/rclcpp/test/test_subscription_traits.cpp @@ -67,13 +67,13 @@ TEST(TestSubscriptionTraits, is_serialized_callback) { // Test regular functions auto cb1 = &serialized_callback_copy; static_assert( - rclcpp::subscription_traits::is_serialized_callback::value == true, - "rcl_serialized_message_t in a first argument callback makes it a serialized callback"); + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a rcl_serialized_message_t * is not a serialized callback"); auto cb2 = &serialized_callback_shared_ptr; static_assert( - rclcpp::subscription_traits::is_serialized_callback::value == true, - "std::shared_ptr in a callback makes it a serialized callback"); + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a std::shared_ptr is not a serialized callback"); auto cb3 = ¬_serialized_callback; static_assert( @@ -90,8 +90,8 @@ TEST(TestSubscriptionTraits, is_serialized_callback) { (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"); + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing rcl_serialized_message_t is not a serialized callback"); using MessageT = test_msgs::msg::Empty; using MessageTAllocator = std::allocator; From 3b892e278aa248f0e714baa3352e943b68230249 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 23 Apr 2020 17:33:56 -0300 Subject: [PATCH 024/121] Ensure logging is initialized just once (#998) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/context.hpp | 3 ++ rclcpp/include/rclcpp/init_options.hpp | 11 +++++ rclcpp/src/rclcpp/context.cpp | 62 +++++++++++++++++++++++++- rclcpp/src/rclcpp/init_options.cpp | 13 ++++++ 4 files changed, 88 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 76d3cb1dca..5d7a6bcecf 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -338,6 +338,9 @@ class Context : public std::enable_shared_from_this rclcpp::InitOptions init_options_; std::string shutdown_reason_; + // Keep shared ownership of global logging configure mutex. + std::shared_ptr logging_configure_mutex_; + std::unordered_map> sub_contexts_; // This mutex is recursive so that the constructor of a sub context may // attempt to acquire another sub context. diff --git a/rclcpp/include/rclcpp/init_options.hpp b/rclcpp/include/rclcpp/init_options.hpp index fd0e4f82d7..7b05ee8d00 100644 --- a/rclcpp/include/rclcpp/init_options.hpp +++ b/rclcpp/include/rclcpp/init_options.hpp @@ -42,6 +42,16 @@ class InitOptions RCLCPP_PUBLIC InitOptions(const InitOptions & other); + /// Return `true` if logging should be initialized when `rclcpp::Context::init` is called. + RCLCPP_PUBLIC + bool + auto_initialize_logging() const; + + /// Set flag indicating if logging should be initialized or not. + RCLCPP_PUBLIC + InitOptions & + auto_initialize_logging(bool initialize_logging); + /// Assignment operator. RCLCPP_PUBLIC InitOptions & @@ -62,6 +72,7 @@ class InitOptions private: std::unique_ptr init_options_; + bool initialize_logging_{true}; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 57937d35fb..1918a77a02 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -22,6 +22,7 @@ #include #include "rcl/init.h" +#include "rcl/logging.h" #include "rclcpp/detail/utilities.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/logging.hpp" @@ -34,8 +35,27 @@ static std::vector> g_contexts; using rclcpp::Context; +static +std::shared_ptr +get_global_logging_configure_mutex() +{ + static auto mutex = std::make_shared(); + return mutex; +} + +static +size_t & +get_logging_reference_count() +{ + static size_t ref_count = 0; + return ref_count; +} + Context::Context() -: rcl_context_(nullptr), shutdown_reason_("") {} +: rcl_context_(nullptr), + shutdown_reason_(""), + logging_configure_mutex_(nullptr) +{} Context::~Context() { @@ -94,6 +114,30 @@ Context::init( rcl_context_.reset(); rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl"); } + + if (init_options.auto_initialize_logging()) { + logging_configure_mutex_ = get_global_logging_configure_mutex(); + if (!logging_configure_mutex_) { + throw std::runtime_error("global logging configure mutex is 'nullptr'"); + } + std::lock_guard guard(*logging_configure_mutex_); + size_t & count = get_logging_reference_count(); + if (0u == count) { + ret = rcl_logging_configure( + &rcl_context_->global_arguments, + rcl_init_options_get_allocator(init_options_.get_rcl_init_options())); + if (RCL_RET_OK != ret) { + rcl_context_.reset(); + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging"); + } + } else { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "logging was initialized more than once"); + } + ++count; + } + try { std::vector unparsed_ros_arguments = detail::get_unparsed_ros_arguments( argc, argv, &(rcl_context_->global_arguments), rcl_get_default_allocator()); @@ -183,6 +227,22 @@ Context::shutdown(const std::string & reason) ++it; } } + // shutdown logger + if (logging_configure_mutex_) { + // logging was initialized by this context + std::lock_guard guard(*logging_configure_mutex_); + size_t & count = get_logging_reference_count(); + if (0u == --count) { + rcl_ret_t rcl_ret = rcl_logging_fini(); + if (RCL_RET_OK != rcl_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__file__) ":" + RCUTILS_STRINGIFY(__LINE__) + " failed to fini logging"); + rcl_reset_error(); + } + } + } return true; } diff --git a/rclcpp/src/rclcpp/init_options.cpp b/rclcpp/src/rclcpp/init_options.cpp index ba46edbec1..ad2a900b5f 100644 --- a/rclcpp/src/rclcpp/init_options.cpp +++ b/rclcpp/src/rclcpp/init_options.cpp @@ -46,6 +46,19 @@ InitOptions::InitOptions(const InitOptions & other) shutdown_on_sigint = other.shutdown_on_sigint; } +bool +InitOptions::auto_initialize_logging() const +{ + return initialize_logging_; +} + +InitOptions & +InitOptions::auto_initialize_logging(bool initialize_logging) +{ + initialize_logging_ = initialize_logging; + return *this; +} + InitOptions & InitOptions::operator=(const InitOptions & other) { From c34723288580852ab82013126fcfced92e9ff280 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Thu, 23 Apr 2020 14:18:51 -0700 Subject: [PATCH 025/121] export targets in a addition to include directories / libraries (#1088) Signed-off-by: Dirk Thomas --- rclcpp/CMakeLists.txt | 12 +++++++----- rclcpp_components/CMakeLists.txt | 10 +++++++--- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 1e0581451b..fbb910533f 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -26,8 +26,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -include_directories(include) - set(${PROJECT_NAME}_SRCS src/rclcpp/any_executable.cpp src/rclcpp/callback_group.cpp @@ -160,10 +158,12 @@ foreach(interface_file ${interface_files}) include/rclcpp/node_interfaces/get_${interface_name}.hpp) endforeach() -include_directories("${CMAKE_CURRENT_BINARY_DIR}/include") - add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SRCS}) +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$" + "$") # specific order: dependents before dependencies ament_target_dependencies(${PROJECT_NAME} "libstatistics_collector" @@ -185,7 +185,7 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "RCLCPP_BUILDING_LIBRARY") install( - TARGETS ${PROJECT_NAME} + TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -194,6 +194,7 @@ install( # specific order: dependents before dependencies ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(${PROJECT_NAME}) ament_export_dependencies(libstatistics_collector) ament_export_dependencies(rcl) @@ -255,6 +256,7 @@ if(BUILD_TESTING) endif() ament_add_gtest(test_function_traits test/test_function_traits.cpp) if(TARGET test_function_traits) + target_include_directories(test_function_traits PUBLIC include) ament_target_dependencies(test_function_traits "rcl_interfaces" "rmw" diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index 3f3f37e939..b4330964f1 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -17,13 +17,14 @@ find_package(composition_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rcpputils REQUIRED) -include_directories(include) - add_library( component_manager SHARED src/component_manager.cpp ) +target_include_directories(component_manager PUBLIC + "$" + "$") ament_target_dependencies(component_manager "ament_index_cpp" "class_loader" @@ -68,6 +69,7 @@ if(BUILD_TESTING) set(components "") add_library(test_component SHARED test/components/test_component.cpp) + target_include_directories(test_component PUBLIC include) ament_target_dependencies(test_component "class_loader" "rclcpp") @@ -102,7 +104,7 @@ if(BUILD_TESTING) endif() install( - TARGETS component_manager + TARGETS component_manager EXPORT component_manager ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -129,6 +131,8 @@ install( # specific order: dependents before dependencies ament_export_include_directories(include) ament_export_libraries(component_manager) +ament_export_targets(component_manager) +ament_export_dependencies(ament_index_cpp) ament_export_dependencies(class_loader) ament_export_dependencies(composition_interfaces) ament_export_dependencies(rclcpp) From 56a7285495fcc4fae652ddd528305701cd828371 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 23 Apr 2020 15:28:45 -0700 Subject: [PATCH 026/121] deprecate redundant namespaces (#1083) * deprecate redundant namespaces, move classes to own files, rename some classes Signed-off-by: William Woodall * fixup Signed-off-by: William Woodall * address review comments Signed-off-by: William Woodall * fix ups since rebase Signed-off-by: William Woodall * avoid deprecation warnings from deprecated functions Signed-off-by: William Woodall * more fixes Signed-off-by: William Woodall * another fixup, after another rebase Signed-off-by: William Woodall --- rclcpp/CMakeLists.txt | 3 +- rclcpp/include/rclcpp/any_executable.hpp | 9 +- rclcpp/include/rclcpp/callback_group.hpp | 9 +- .../rclcpp/contexts/default_context.hpp | 17 +- rclcpp/include/rclcpp/create_client.hpp | 2 +- rclcpp/include/rclcpp/create_service.hpp | 2 +- rclcpp/include/rclcpp/create_timer.hpp | 6 +- rclcpp/include/rclcpp/exceptions.hpp | 261 +--------------- .../include/rclcpp/exceptions/exceptions.hpp | 279 ++++++++++++++++++ rclcpp/include/rclcpp/executor.hpp | 59 +--- rclcpp/include/rclcpp/executor_options.hpp | 57 ++++ rclcpp/include/rclcpp/executors.hpp | 12 +- .../executors/multi_threaded_executor.hpp | 6 +- .../executors/single_threaded_executor.hpp | 22 +- .../static_executor_entities_collector.hpp | 4 +- .../static_single_threaded_executor.hpp | 16 +- .../{ => experimental}/executable_list.hpp | 10 +- rclcpp/include/rclcpp/future_return_code.hpp | 61 ++++ rclcpp/include/rclcpp/guard_condition.hpp | 2 +- rclcpp/include/rclcpp/memory_strategy.hpp | 22 +- rclcpp/include/rclcpp/node.hpp | 14 +- rclcpp/include/rclcpp/node_impl.hpp | 6 +- .../rclcpp/node_interfaces/node_base.hpp | 14 +- .../node_interfaces/node_base_interface.hpp | 10 +- .../rclcpp/node_interfaces/node_services.hpp | 4 +- .../node_services_interface.hpp | 4 +- .../rclcpp/node_interfaces/node_timers.hpp | 2 +- .../node_interfaces/node_timers_interface.hpp | 2 +- .../rclcpp/node_interfaces/node_topics.hpp | 4 +- .../node_interfaces/node_topics_interface.hpp | 4 +- .../rclcpp/node_interfaces/node_waitables.hpp | 4 +- .../node_waitables_interface.hpp | 4 +- rclcpp/include/rclcpp/node_options.hpp | 4 +- rclcpp/include/rclcpp/parameter_client.hpp | 14 +- rclcpp/include/rclcpp/publisher_options.hpp | 5 +- rclcpp/include/rclcpp/rclcpp.hpp | 2 +- .../strategies/allocator_memory_strategy.hpp | 10 +- .../message_pool_memory_strategy.hpp | 2 + .../include/rclcpp/subscription_options.hpp | 2 +- rclcpp/include/rclcpp/utilities.hpp | 2 +- rclcpp/include/rclcpp/wait_set_template.hpp | 2 +- rclcpp/src/rclcpp/any_executable.cpp | 2 +- rclcpp/src/rclcpp/callback_group.cpp | 4 +- .../src/rclcpp/contexts/default_context.cpp | 4 +- .../rclcpp/{ => exceptions}/exceptions.cpp | 0 rclcpp/src/rclcpp/executable_list.cpp | 4 +- rclcpp/src/rclcpp/executor.cpp | 50 +--- .../executors/multi_threaded_executor.cpp | 6 +- .../executors/single_threaded_executor.cpp | 6 +- .../static_executor_entities_collector.cpp | 2 +- .../static_single_threaded_executor.cpp | 6 +- rclcpp/src/rclcpp/future_return_code.cpp | 50 ++++ rclcpp/src/rclcpp/memory_strategy.cpp | 12 +- rclcpp/src/rclcpp/node.cpp | 9 +- .../src/rclcpp/node_interfaces/node_base.cpp | 16 +- .../rclcpp/node_interfaces/node_services.cpp | 4 +- .../rclcpp/node_interfaces/node_timers.cpp | 2 +- .../rclcpp/node_interfaces/node_topics.cpp | 4 +- .../rclcpp/node_interfaces/node_waitables.cpp | 4 +- rclcpp/src/rclcpp/parameter_client.cpp | 22 +- rclcpp/src/rclcpp/timer.cpp | 2 +- rclcpp/src/rclcpp/utilities.cpp | 10 +- .../test_multi_threaded_executor.cpp | 4 +- rclcpp/test/test_interface_traits.cpp | 2 +- rclcpp/test/test_utilities.cpp | 4 +- .../include/rclcpp_action/create_client.hpp | 8 +- .../include/rclcpp_action/create_server.hpp | 6 +- rclcpp_action/test/test_server.cpp | 8 +- .../rclcpp_components/component_manager.hpp | 4 +- rclcpp_components/src/component_manager.cpp | 2 +- .../test/test_component_manager_api.cpp | 20 +- .../rclcpp_lifecycle/lifecycle_node.hpp | 14 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 6 +- rclcpp_lifecycle/src/lifecycle_node.cpp | 8 +- 74 files changed, 716 insertions(+), 563 deletions(-) create mode 100644 rclcpp/include/rclcpp/exceptions/exceptions.hpp create mode 100644 rclcpp/include/rclcpp/executor_options.hpp rename rclcpp/include/rclcpp/{ => experimental}/executable_list.hpp (92%) create mode 100644 rclcpp/include/rclcpp/future_return_code.hpp rename rclcpp/src/rclcpp/{ => exceptions}/exceptions.cpp (100%) create mode 100644 rclcpp/src/rclcpp/future_return_code.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index fbb910533f..518951017b 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -39,7 +39,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/detail/utilities.cpp src/rclcpp/duration.cpp src/rclcpp/event.cpp - src/rclcpp/exceptions.cpp + src/rclcpp/exceptions/exceptions.cpp src/rclcpp/executable_list.cpp src/rclcpp/executor.cpp src/rclcpp/executors.cpp @@ -48,6 +48,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/executors/single_threaded_executor.cpp src/rclcpp/executors/static_executor_entities_collector.cpp src/rclcpp/executors/static_single_threaded_executor.cpp + src/rclcpp/future_return_code.cpp src/rclcpp/graph_listener.cpp src/rclcpp/guard_condition.cpp src/rclcpp/init_options.cpp diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index aae25dca78..675d46554f 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -29,8 +29,6 @@ namespace rclcpp { -namespace executor -{ struct AnyExecutable { @@ -47,10 +45,15 @@ struct AnyExecutable rclcpp::ClientBase::SharedPtr client; rclcpp::Waitable::SharedPtr waitable; // These are used to keep the scope on the containing items - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; + rclcpp::CallbackGroup::SharedPtr callback_group; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; }; +namespace executor +{ + +using AnyExecutable [[deprecated("use rclcpp::AnyExecutable instead")]] = AnyExecutable; + } // namespace executor } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index ef87a2f032..5e1157f204 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -40,9 +40,6 @@ class NodeTopics; class NodeWaitables; } // namespace node_interfaces -namespace callback_group -{ - enum class CallbackGroupType { MutuallyExclusive, @@ -162,6 +159,12 @@ class CallbackGroup } }; +namespace callback_group +{ + +using CallbackGroupType [[deprecated("use rclcpp::CallbackGroupType instead")]] = CallbackGroupType; +using CallbackGroup [[deprecated("use rclcpp::CallbackGroup instead")]] = CallbackGroup; + } // namespace callback_group } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/contexts/default_context.hpp b/rclcpp/include/rclcpp/contexts/default_context.hpp index 2ed9bcbbf1..0c2bbf8265 100644 --- a/rclcpp/include/rclcpp/contexts/default_context.hpp +++ b/rclcpp/include/rclcpp/contexts/default_context.hpp @@ -22,8 +22,6 @@ namespace rclcpp { namespace contexts { -namespace default_context -{ class DefaultContext : public rclcpp::Context { @@ -38,6 +36,21 @@ RCLCPP_PUBLIC DefaultContext::SharedPtr get_global_default_context(); +namespace default_context +{ + +using DefaultContext +[[deprecated("use rclcpp::contexts::DefaultContext instead")]] = DefaultContext; + +[[deprecated("use rclcpp::contexts::get_global_default_context() instead")]] +RCLCPP_PUBLIC +inline +DefaultContext::SharedPtr +get_global_default_context() +{ + return rclcpp::contexts::get_global_default_context(); +} + } // namespace default_context } // namespace contexts } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/create_client.hpp b/rclcpp/include/rclcpp/create_client.hpp index 94f6b212fa..1a960b8d8f 100644 --- a/rclcpp/include/rclcpp/create_client.hpp +++ b/rclcpp/include/rclcpp/create_client.hpp @@ -35,7 +35,7 @@ create_client( std::shared_ptr node_services, const std::string & service_name, const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { rcl_client_options_t options = rcl_client_get_default_options(); options.qos = qos_profile; diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp index ac7846239d..9aaa02a1ed 100644 --- a/rclcpp/include/rclcpp/create_service.hpp +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -37,7 +37,7 @@ create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { rclcpp::AnyServiceCallback any_service_callback; any_service_callback.set(std::forward(callback)); diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index ed2965b01c..f7b810dc4d 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -39,7 +39,7 @@ create_timer( rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr) { auto timer = rclcpp::GenericTimer::make_shared( clock, @@ -59,7 +59,7 @@ create_timer( rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr) { return create_timer( rclcpp::node_interfaces::get_node_base_interface(node), @@ -90,7 +90,7 @@ typename rclcpp::WallTimer::SharedPtr create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group, + rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, node_interfaces::NodeTimersInterface * node_timers) { diff --git a/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp/include/rclcpp/exceptions.hpp index 4fe5988113..dcac558512 100644 --- a/rclcpp/include/rclcpp/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions.hpp @@ -15,265 +15,6 @@ #ifndef RCLCPP__EXCEPTIONS_HPP_ #define RCLCPP__EXCEPTIONS_HPP_ -#include -#include -#include - -#include "rcl/error_handling.h" -#include "rcl/types.h" -#include "rclcpp/visibility_control.hpp" - -#include "rcpputils/join.hpp" - -namespace rclcpp -{ -namespace exceptions -{ - -/// Thrown when a method is trying to use a node, but it is invalid. -class InvalidNodeError : public std::runtime_error -{ -public: - InvalidNodeError() - : std::runtime_error("node is invalid") {} -}; - -/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid. -class NameValidationError : public std::invalid_argument -{ -public: - NameValidationError( - 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_) - {} - - 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; - const std::string error_msg; - const size_t invalid_index; -}; - -/// Thrown when a node name is invalid. -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. -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. -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. -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. -/** - * Passing nullptr for reset_error is safe and will avoid calling any function - * to reset the error. - * - * \param ret the return code for the current error state - * \param prefix string to prefix to the error if applicable (not all errors have custom messages) - * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used - * \param reset_error function to be called before throwing which whill clear the error state - * \throws std::invalid_argument if ret is RCL_RET_OK - * \throws std::runtime_error if the rcl_get_error_state returns 0 - * \throws RCLErrorBase some child class exception based on ret - */ -/* *INDENT-OFF* */ // Uncrustify cannot yet understand [[noreturn]] properly -RCLCPP_PUBLIC -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); -/* *INDENT-ON* */ - -class RCLErrorBase -{ -public: - RCLCPP_PUBLIC - RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state); - virtual ~RCLErrorBase() {} - - rcl_ret_t ret; - std::string message; - std::string file; - size_t line; - std::string formatted_message; -}; - -/// Created when the return code does not match one of the other specialized exceptions. -class RCLError : public RCLErrorBase, public std::runtime_error -{ -public: - RCLCPP_PUBLIC - RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix); - RCLCPP_PUBLIC - RCLError(const RCLErrorBase & base_exc, const std::string & prefix); -}; - -/// Created when the ret is RCL_RET_BAD_ALLOC. -class RCLBadAlloc : public RCLErrorBase, public std::bad_alloc -{ -public: - RCLCPP_PUBLIC - RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state); - RCLCPP_PUBLIC - explicit RCLBadAlloc(const RCLErrorBase & base_exc); -}; - -/// Created when the ret is RCL_RET_INVALID_ARGUMENT. -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); - RCLCPP_PUBLIC - RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix); -}; - -/// Created when the ret is RCL_RET_INVALID_ROS_ARGS. -class RCLInvalidROSArgsError : public RCLErrorBase, public std::runtime_error -{ -public: - RCLCPP_PUBLIC - RCLInvalidROSArgsError( - rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix); - RCLCPP_PUBLIC - RCLInvalidROSArgsError(const RCLErrorBase & base_exc, const std::string & prefix); -}; - -/// Thrown when unparsed ROS specific arguments are found. -class UnknownROSArgsError : public std::runtime_error -{ -public: - explicit UnknownROSArgsError(std::vector && unknown_ros_args_in) - : std::runtime_error( - "found unknown ROS arguments: '" + rcpputils::join(unknown_ros_args_in, "', '") + "'"), - unknown_ros_args(unknown_ros_args_in) - { - } - - const std::vector unknown_ros_args; -}; - -/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered. -class InvalidEventError : public std::runtime_error -{ -public: - 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") {} -}; - -/// Thrown if passed parameters are inconsistent or invalid -class InvalidParametersException : public std::runtime_error -{ -public: - // Inherit constructors from runtime_error. - using std::runtime_error::runtime_error; -}; - -/// Thrown if passed parameter value is invalid. -class InvalidParameterValueException : public std::runtime_error -{ - // Inherit constructors from runtime_error. - using std::runtime_error::runtime_error; -}; - -/// Thrown if requested parameter type is invalid. -/** - * Essentially the same as rclcpp::ParameterTypeException, but with parameter - * name in the error message. - */ -class InvalidParameterTypeException : public std::runtime_error -{ -public: - /// Construct an instance. - /** - * \param[in] name the name of the parameter. - * \param[in] message custom exception message. - */ - RCLCPP_PUBLIC - InvalidParameterTypeException(const std::string & name, const std::string message) - : std::runtime_error("parameter '" + name + "' has invalid type: " + message) - {} -}; - -/// Thrown if parameter is already declared. -class ParameterAlreadyDeclaredException : public std::runtime_error -{ - // Inherit constructors from runtime_error. - using std::runtime_error::runtime_error; -}; - -/// Thrown if parameter is not declared, e.g. either set or get was called without first declaring. -class ParameterNotDeclaredException : public std::runtime_error -{ - // Inherit constructors from runtime_error. - using std::runtime_error::runtime_error; -}; - -/// Thrown if parameter is immutable and therefore cannot be undeclared. -class ParameterImmutableException : public std::runtime_error -{ - // Inherit constructors from runtime_error. - using std::runtime_error::runtime_error; -}; - -/// Thrown if parameter is modified while in a set callback. -class ParameterModifiedInCallbackException : public std::runtime_error -{ - // Inherit constructors from runtime_error. - using std::runtime_error::runtime_error; -}; - -} // namespace exceptions -} // namespace rclcpp +#include "rclcpp/exceptions/exceptions.hpp" #endif // RCLCPP__EXCEPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp new file mode 100644 index 0000000000..d501acd71c --- /dev/null +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -0,0 +1,279 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_ +#define RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_ + +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/types.h" +#include "rclcpp/visibility_control.hpp" + +#include "rcpputils/join.hpp" + +namespace rclcpp +{ +namespace exceptions +{ + +/// Thrown when a method is trying to use a node, but it is invalid. +class InvalidNodeError : public std::runtime_error +{ +public: + InvalidNodeError() + : std::runtime_error("node is invalid") {} +}; + +/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid. +class NameValidationError : public std::invalid_argument +{ +public: + NameValidationError( + 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_) + {} + + 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; + const std::string error_msg; + const size_t invalid_index; +}; + +/// Thrown when a node name is invalid. +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. +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. +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. +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. +/** + * Passing nullptr for reset_error is safe and will avoid calling any function + * to reset the error. + * + * \param ret the return code for the current error state + * \param prefix string to prefix to the error if applicable (not all errors have custom messages) + * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used + * \param reset_error function to be called before throwing which whill clear the error state + * \throws std::invalid_argument if ret is RCL_RET_OK + * \throws std::runtime_error if the rcl_get_error_state returns 0 + * \throws RCLErrorBase some child class exception based on ret + */ +/* *INDENT-OFF* */ // Uncrustify cannot yet understand [[noreturn]] properly +RCLCPP_PUBLIC +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); +/* *INDENT-ON* */ + +class RCLErrorBase +{ +public: + RCLCPP_PUBLIC + RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state); + virtual ~RCLErrorBase() {} + + rcl_ret_t ret; + std::string message; + std::string file; + size_t line; + std::string formatted_message; +}; + +/// Created when the return code does not match one of the other specialized exceptions. +class RCLError : public RCLErrorBase, public std::runtime_error +{ +public: + RCLCPP_PUBLIC + RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix); + RCLCPP_PUBLIC + RCLError(const RCLErrorBase & base_exc, const std::string & prefix); +}; + +/// Created when the ret is RCL_RET_BAD_ALLOC. +class RCLBadAlloc : public RCLErrorBase, public std::bad_alloc +{ +public: + RCLCPP_PUBLIC + RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state); + RCLCPP_PUBLIC + explicit RCLBadAlloc(const RCLErrorBase & base_exc); +}; + +/// Created when the ret is RCL_RET_INVALID_ARGUMENT. +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); + RCLCPP_PUBLIC + RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix); +}; + +/// Created when the ret is RCL_RET_INVALID_ROS_ARGS. +class RCLInvalidROSArgsError : public RCLErrorBase, public std::runtime_error +{ +public: + RCLCPP_PUBLIC + RCLInvalidROSArgsError( + rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix); + RCLCPP_PUBLIC + RCLInvalidROSArgsError(const RCLErrorBase & base_exc, const std::string & prefix); +}; + +/// Thrown when unparsed ROS specific arguments are found. +class UnknownROSArgsError : public std::runtime_error +{ +public: + explicit UnknownROSArgsError(std::vector && unknown_ros_args_in) + : std::runtime_error( + "found unknown ROS arguments: '" + rcpputils::join(unknown_ros_args_in, "', '") + "'"), + unknown_ros_args(unknown_ros_args_in) + { + } + + const std::vector unknown_ros_args; +}; + +/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered. +class InvalidEventError : public std::runtime_error +{ +public: + 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") {} +}; + +/// Thrown if passed parameters are inconsistent or invalid +class InvalidParametersException : public std::runtime_error +{ +public: + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if passed parameter value is invalid. +class InvalidParameterValueException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if requested parameter type is invalid. +/** + * Essentially the same as rclcpp::ParameterTypeException, but with parameter + * name in the error message. + */ +class InvalidParameterTypeException : public std::runtime_error +{ +public: + /// Construct an instance. + /** + * \param[in] name the name of the parameter. + * \param[in] message custom exception message. + */ + RCLCPP_PUBLIC + InvalidParameterTypeException(const std::string & name, const std::string message) + : std::runtime_error("parameter '" + name + "' has invalid type: " + message) + {} +}; + +/// Thrown if parameter is already declared. +class ParameterAlreadyDeclaredException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if parameter is not declared, e.g. either set or get was called without first declaring. +class ParameterNotDeclaredException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if parameter is immutable and therefore cannot be undeclared. +class ParameterImmutableException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if parameter is modified while in a set callback. +class ParameterModifiedInCallbackException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +} // namespace exceptions +} // namespace rclcpp + +#endif // RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index aa94c01476..4f71ae0052 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -30,6 +30,8 @@ #include "rcl/wait.h" #include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/executor_options.hpp" +#include "rclcpp/future_return_code.hpp" #include "rclcpp/memory_strategies.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" @@ -42,48 +44,6 @@ namespace rclcpp // Forward declaration is used in convenience method signature. class Node; -namespace executor -{ - -/// Return codes to be used with spin_until_future_complete. -/** - * SUCCESS: The future is complete and can be accessed with "get" without blocking. - * This does not indicate that the operation succeeded; "get" may still throw an exception. - * 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}; - -RCLCPP_PUBLIC -std::ostream & -operator<<(std::ostream & os, const FutureReturnCode & future_return_code); - -RCLCPP_PUBLIC -std::string -to_string(const FutureReturnCode & future_return_code); - -/// -/** - * Options to be passed to the executor constructor. - */ -struct ExecutorArgs -{ - 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; - size_t max_conditions; -}; - -static inline ExecutorArgs create_default_executor_arguments() -{ - return ExecutorArgs(); -} - /// Coordinate the order and timing of available communication tasks. /** * Executor provides spin functions (including spin_node_once and spin_some). @@ -100,9 +60,11 @@ class Executor RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor) /// Default constructor. - // \param[in] ms The memory strategy to be used with this executor. + /** + * \param[in] options Options used to configure the executor. + */ RCLCPP_PUBLIC - explicit Executor(const ExecutorArgs & args = ExecutorArgs()); + explicit Executor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); /// Default destructor. RCLCPP_PUBLIC @@ -323,10 +285,10 @@ class Executor RCLCPP_PUBLIC rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group); + get_node_by_group(rclcpp::CallbackGroup::SharedPtr group); RCLCPP_PUBLIC - rclcpp::callback_group::CallbackGroup::SharedPtr + rclcpp::CallbackGroup::SharedPtr get_group_by_timer(rclcpp::TimerBase::SharedPtr timer); RCLCPP_PUBLIC @@ -363,6 +325,11 @@ class Executor std::list guard_conditions_; }; +namespace executor +{ + +using Executor [[deprecated("use rclcpp::Executor instead")]] = rclcpp::Executor; + } // namespace executor } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/executor_options.hpp b/rclcpp/include/rclcpp/executor_options.hpp new file mode 100644 index 0000000000..d5ec6d0da9 --- /dev/null +++ b/rclcpp/include/rclcpp/executor_options.hpp @@ -0,0 +1,57 @@ +// Copyright 2014-2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXECUTOR_OPTIONS_HPP_ +#define RCLCPP__EXECUTOR_OPTIONS_HPP_ + +#include "rclcpp/context.hpp" +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/memory_strategies.hpp" +#include "rclcpp/memory_strategy.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Options to be passed to the executor constructor. +struct ExecutorOptions +{ + ExecutorOptions() + : memory_strategy(rclcpp::memory_strategies::create_default_strategy()), + context(rclcpp::contexts::get_global_default_context()), + max_conditions(0) + {} + + rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy; + rclcpp::Context::SharedPtr context; + size_t max_conditions; +}; + +namespace executor +{ + +using ExecutorArgs [[deprecated("use rclcpp::ExecutorOptions instead")]] = ExecutorOptions; + +[[deprecated("use rclcpp::ExecutorOptions() instead")]] +inline +rclcpp::ExecutorOptions +create_default_executor_arguments() +{ + return rclcpp::ExecutorOptions(); +} + +} // namespace executor +} // namespace rclcpp + +#endif // RCLCPP__EXECUTOR_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index ac91141a43..ec55d1309e 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -67,9 +67,9 @@ using rclcpp::executors::SingleThreadedExecutor; * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ template -rclcpp::executor::FutureReturnCode +rclcpp::FutureReturnCode spin_node_until_future_complete( - rclcpp::executor::Executor & executor, + rclcpp::Executor & executor, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, const std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) @@ -84,9 +84,9 @@ spin_node_until_future_complete( template -rclcpp::executor::FutureReturnCode +rclcpp::FutureReturnCode spin_node_until_future_complete( - rclcpp::executor::Executor & executor, + rclcpp::Executor & executor, std::shared_ptr node_ptr, const std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) @@ -101,7 +101,7 @@ spin_node_until_future_complete( } // namespace executors template -rclcpp::executor::FutureReturnCode +rclcpp::FutureReturnCode spin_until_future_complete( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, const std::shared_future & future, @@ -113,7 +113,7 @@ spin_until_future_complete( template -rclcpp::executor::FutureReturnCode +rclcpp::FutureReturnCode spin_until_future_complete( std::shared_ptr node_ptr, const std::shared_future & future, diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index fd9ad33cf0..7cc82c88db 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -32,7 +32,7 @@ namespace rclcpp namespace executors { -class MultiThreadedExecutor : public executor::Executor +class MultiThreadedExecutor : public rclcpp::Executor { public: RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor) @@ -45,14 +45,14 @@ class MultiThreadedExecutor : public executor::Executor * This is useful for reproducing some bugs related to taking work more than * once. * - * \param args common arguments for all executors + * \param options common options for all executors * \param number_of_threads number of threads to have in the thread pool, * the default 0 will use the number of cpu cores found instead * \param yield_before_execute if true std::this_thread::yield() is called */ RCLCPP_PUBLIC MultiThreadedExecutor( - const executor::ExecutorArgs & args = executor::ExecutorArgs(), + const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(), size_t number_of_threads = 0, bool yield_before_execute = false, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp index 523c2e5657..be1310f8df 100644 --- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -35,25 +35,31 @@ namespace rclcpp namespace executors { -/// Single-threaded executor implementation -// This is the default executor created by rclcpp::spin. -class SingleThreadedExecutor : public executor::Executor +/// Single-threaded executor implementation. +/** + * This is the default executor created by rclcpp::spin. + */ +class SingleThreadedExecutor : public rclcpp::Executor { public: RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor) /// Default constructor. See the default constructor for Executor. RCLCPP_PUBLIC - SingleThreadedExecutor( - const executor::ExecutorArgs & args = executor::ExecutorArgs()); + explicit SingleThreadedExecutor( + const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); - /// Default destrcutor. + /// Default destructor. RCLCPP_PUBLIC virtual ~SingleThreadedExecutor(); /// Single-threaded implementation of spin. - // 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). + /** + * This function will block until work comes in, execute it, and then repeat + * the process until canceled. + * It may be interrupt by a call to rclcpp::Executor::cancel() or by ctrl-c + * if the associated context is configured to shutdown on SIGINT. + */ RCLCPP_PUBLIC void spin() override; diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index c2792afae6..f9a0525112 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp @@ -22,7 +22,7 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" -#include "rclcpp/executable_list.hpp" +#include "rclcpp/experimental/executable_list.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/visibility_control.hpp" @@ -159,7 +159,7 @@ class StaticExecutorEntitiesCollector final rcl_wait_set_t * p_wait_set_ = nullptr; /// Executable list: timers, subscribers, clients, services and waitables - rclcpp::executor::ExecutableList exec_list_; + rclcpp::experimental::ExecutableList exec_list_; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 43d086f7db..bf68985812 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -23,9 +23,9 @@ #include "rmw/rmw.h" -#include "rclcpp/executable_list.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/executors/static_executor_entities_collector.hpp" +#include "rclcpp/experimental/executable_list.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/memory_strategies.hpp" #include "rclcpp/node.hpp" @@ -54,7 +54,7 @@ namespace executors * exec.spin(); * exec.remove_node(node); */ -class StaticSingleThreadedExecutor : public executor::Executor +class StaticSingleThreadedExecutor : public rclcpp::Executor { public: RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor) @@ -62,7 +62,7 @@ class StaticSingleThreadedExecutor : public executor::Executor /// Default constructor. See the default constructor for Executor. RCLCPP_PUBLIC explicit StaticSingleThreadedExecutor( - const executor::ExecutorArgs & args = executor::ExecutorArgs()); + const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); /// Default destrcutor. RCLCPP_PUBLIC @@ -131,14 +131,14 @@ class StaticSingleThreadedExecutor : public executor::Executor * exec.spin_until_future_complete(future); */ template - rclcpp::executor::FutureReturnCode + rclcpp::FutureReturnCode spin_until_future_complete( std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) { std::future_status status = future.wait_for(std::chrono::seconds(0)); if (status == std::future_status::ready) { - return rclcpp::executor::FutureReturnCode::SUCCESS; + return rclcpp::FutureReturnCode::SUCCESS; } auto end_time = std::chrono::steady_clock::now(); @@ -159,7 +159,7 @@ class StaticSingleThreadedExecutor : public executor::Executor // Check if the future is set, return SUCCESS if it is. status = future.wait_for(std::chrono::seconds(0)); if (status == std::future_status::ready) { - return rclcpp::executor::FutureReturnCode::SUCCESS; + return rclcpp::FutureReturnCode::SUCCESS; } // If the original timeout is < 0, then this is blocking, never TIMEOUT. if (timeout_ns < std::chrono::nanoseconds::zero()) { @@ -168,14 +168,14 @@ class StaticSingleThreadedExecutor : public executor::Executor // Otherwise check if we still have time to wait, return TIMEOUT if not. auto now = std::chrono::steady_clock::now(); if (now >= end_time) { - return rclcpp::executor::FutureReturnCode::TIMEOUT; + return rclcpp::FutureReturnCode::TIMEOUT; } // Subtract the elapsed time from the original timeout. timeout_left = std::chrono::duration_cast(end_time - now); } // The future did not complete before ok() returned false, return INTERRUPTED. - return rclcpp::executor::FutureReturnCode::INTERRUPTED; + return rclcpp::FutureReturnCode::INTERRUPTED; } protected: diff --git a/rclcpp/include/rclcpp/executable_list.hpp b/rclcpp/include/rclcpp/experimental/executable_list.hpp similarity index 92% rename from rclcpp/include/rclcpp/executable_list.hpp rename to rclcpp/include/rclcpp/experimental/executable_list.hpp index 2929b5410a..e1c70db3de 100644 --- a/rclcpp/include/rclcpp/executable_list.hpp +++ b/rclcpp/include/rclcpp/experimental/executable_list.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__EXECUTABLE_LIST_HPP_ -#define RCLCPP__EXECUTABLE_LIST_HPP_ +#ifndef RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_ +#define RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_ #include #include @@ -27,7 +27,7 @@ namespace rclcpp { -namespace executor +namespace experimental { /// This class contains subscriptionbase, timerbase, etc. which can be used to run callbacks. @@ -86,7 +86,7 @@ class ExecutableList final size_t number_of_waitables; }; -} // namespace executor +} // namespace experimental } // namespace rclcpp -#endif // RCLCPP__EXECUTABLE_LIST_HPP_ +#endif // RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_ diff --git a/rclcpp/include/rclcpp/future_return_code.hpp b/rclcpp/include/rclcpp/future_return_code.hpp new file mode 100644 index 0000000000..9161fa832d --- /dev/null +++ b/rclcpp/include/rclcpp/future_return_code.hpp @@ -0,0 +1,61 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__FUTURE_RETURN_CODE_HPP_ +#define RCLCPP__FUTURE_RETURN_CODE_HPP_ + +#include +#include + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Return codes to be used with spin_until_future_complete. +/** + * SUCCESS: The future is complete and can be accessed with "get" without blocking. + * This does not indicate that the operation succeeded; "get" may still throw an exception. + * 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}; + +/// Stream operator for FutureReturnCode. +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, const FutureReturnCode & future_return_code); + +/// String conversion function for FutureReturnCode. +RCLCPP_PUBLIC +std::string +to_string(const FutureReturnCode & future_return_code); + +namespace executor +{ + +using FutureReturnCode [[deprecated("use rclcpp::FutureReturnCode instead")]] = FutureReturnCode; + +[[deprecated("use rclcpp::to_string(const rclcpp::FutureReturnCode &) instead")]] +inline +std::string +to_string(const rclcpp::FutureReturnCode & future_return_code) +{ + return rclcpp::to_string(future_return_code); +} + +} // namespace executor +} // namespace rclcpp + +#endif // RCLCPP__FUTURE_RETURN_CODE_HPP_ diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 5088181022..010c127a07 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -47,7 +47,7 @@ class GuardCondition RCLCPP_PUBLIC explicit GuardCondition( rclcpp::Context::SharedPtr context = - rclcpp::contexts::default_context::get_global_default_context()); + rclcpp::contexts::get_global_default_context()); RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 3549802bdd..5677533a4a 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -67,27 +67,27 @@ class RCLCPP_PUBLIC MemoryStrategy virtual void get_next_subscription( - rclcpp::executor::AnyExecutable & any_exec, + rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) = 0; virtual void get_next_service( - rclcpp::executor::AnyExecutable & any_exec, + rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) = 0; virtual void get_next_client( - rclcpp::executor::AnyExecutable & any_exec, + rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) = 0; virtual void get_next_timer( - rclcpp::executor::AnyExecutable & any_exec, + rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) = 0; virtual void get_next_waitable( - rclcpp::executor::AnyExecutable & any_exec, + rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) = 0; virtual rcl_allocator_t @@ -115,30 +115,30 @@ class RCLCPP_PUBLIC MemoryStrategy static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group( - rclcpp::callback_group::CallbackGroup::SharedPtr group, + rclcpp::CallbackGroup::SharedPtr group, const WeakNodeList & weak_nodes); - static rclcpp::callback_group::CallbackGroup::SharedPtr + static rclcpp::CallbackGroup::SharedPtr get_group_by_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, const WeakNodeList & weak_nodes); - static rclcpp::callback_group::CallbackGroup::SharedPtr + static rclcpp::CallbackGroup::SharedPtr get_group_by_service( rclcpp::ServiceBase::SharedPtr service, const WeakNodeList & weak_nodes); - static rclcpp::callback_group::CallbackGroup::SharedPtr + static rclcpp::CallbackGroup::SharedPtr get_group_by_client( rclcpp::ClientBase::SharedPtr client, const WeakNodeList & weak_nodes); - static rclcpp::callback_group::CallbackGroup::SharedPtr + static rclcpp::CallbackGroup::SharedPtr get_group_by_timer( rclcpp::TimerBase::SharedPtr timer, const WeakNodeList & weak_nodes); - static rclcpp::callback_group::CallbackGroup::SharedPtr + static rclcpp::CallbackGroup::SharedPtr get_group_by_waitable( rclcpp::Waitable::SharedPtr waitable, const WeakNodeList & weak_nodes); diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 43e3a94fdc..6131fef95a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -135,12 +135,12 @@ class Node : public std::enable_shared_from_this /// Create and return a callback group. RCLCPP_PUBLIC - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + rclcpp::CallbackGroup::SharedPtr + create_callback_group(rclcpp::CallbackGroupType group_type); /// Return the list of callback groups in the node. RCLCPP_PUBLIC - const std::vector & + const std::vector & get_callback_groups() const; /// Create and return a Publisher. @@ -227,7 +227,7 @@ class Node : public std::enable_shared_from_this create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Client. */ template @@ -235,7 +235,7 @@ class Node : public std::enable_shared_from_this 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); + rclcpp::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Service. */ template @@ -244,7 +244,7 @@ class Node : public std::enable_shared_from_this const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Declare and initialize a parameter, return the effective value. /** @@ -1142,7 +1142,7 @@ class Node : public std::enable_shared_from_this RCLCPP_PUBLIC bool - group_in_node(callback_group::CallbackGroup::SharedPtr group); + group_in_node(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 512f472f79..4a24d0d359 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -109,7 +109,7 @@ typename rclcpp::WallTimer::SharedPtr Node::create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { return rclcpp::create_wall_timer( period, @@ -124,7 +124,7 @@ typename Client::SharedPtr Node::create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { return rclcpp::create_client( node_base_, @@ -141,7 +141,7 @@ Node::create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { return rclcpp::create_service( node_base_, diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index c21514ff97..73861df680 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -96,22 +96,22 @@ class NodeBase : public NodeBaseInterface RCLCPP_PUBLIC - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) override; + rclcpp::CallbackGroup::SharedPtr + create_callback_group(rclcpp::CallbackGroupType group_type) override; RCLCPP_PUBLIC - rclcpp::callback_group::CallbackGroup::SharedPtr + rclcpp::CallbackGroup::SharedPtr get_default_callback_group() override; RCLCPP_PUBLIC bool - callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) override; + callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) override; RCLCPP_PUBLIC - const std::vector & + const std::vector & get_callback_groups() const override; RCLCPP_PUBLIC @@ -146,8 +146,8 @@ class NodeBase : public NodeBaseInterface std::shared_ptr node_handle_; - rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_; - std::vector callback_groups_; + rclcpp::CallbackGroup::SharedPtr default_callback_group_; + std::vector callback_groups_; std::atomic_bool associated_with_executor_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index eb7d34d81c..33e4ee3462 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -111,25 +111,25 @@ class NodeBaseInterface /// Create and return a callback group. RCLCPP_PUBLIC virtual - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) = 0; + rclcpp::CallbackGroup::SharedPtr + create_callback_group(rclcpp::CallbackGroupType group_type) = 0; /// Return the default callback group. RCLCPP_PUBLIC virtual - rclcpp::callback_group::CallbackGroup::SharedPtr + rclcpp::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; + callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) = 0; /// Return list of callback groups associated with this node. RCLCPP_PUBLIC virtual - const std::vector & + const std::vector & get_callback_groups() const = 0; /// Return the atomic bool which is used to ensure only one executor is used. diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp index 416fb32c7e..01c9912344 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp @@ -46,14 +46,14 @@ class NodeServices : public NodeServicesInterface void add_client( rclcpp::ClientBase::SharedPtr client_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) override; + rclcpp::CallbackGroup::SharedPtr group) override; RCLCPP_PUBLIC void add_service( rclcpp::ServiceBase::SharedPtr service_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) override; + rclcpp::CallbackGroup::SharedPtr group) override; private: RCLCPP_DISABLE_COPY(NodeServices) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp index c75fec865f..53a2e3fc4c 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp @@ -41,14 +41,14 @@ class NodeServicesInterface void add_client( rclcpp::ClientBase::SharedPtr client_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + rclcpp::CallbackGroup::SharedPtr group) = 0; RCLCPP_PUBLIC virtual void add_service( rclcpp::ServiceBase::SharedPtr service_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + rclcpp::CallbackGroup::SharedPtr group) = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp index 27e2292dbe..03f151ff25 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp @@ -46,7 +46,7 @@ class NodeTimers : public NodeTimersInterface void add_timer( rclcpp::TimerBase::SharedPtr timer, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override; + rclcpp::CallbackGroup::SharedPtr callback_group) override; private: RCLCPP_DISABLE_COPY(NodeTimers) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp index 422409af57..4573d3d02b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp @@ -41,7 +41,7 @@ class NodeTimersInterface void add_timer( rclcpp::TimerBase::SharedPtr timer, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; + rclcpp::CallbackGroup::SharedPtr callback_group) = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index 227d193516..38402eff6e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -58,7 +58,7 @@ class NodeTopics : public NodeTopicsInterface void add_publisher( rclcpp::PublisherBase::SharedPtr publisher, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override; + rclcpp::CallbackGroup::SharedPtr callback_group) override; RCLCPP_PUBLIC rclcpp::SubscriptionBase::SharedPtr @@ -71,7 +71,7 @@ class NodeTopics : public NodeTopicsInterface void add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override; + rclcpp::CallbackGroup::SharedPtr callback_group) override; RCLCPP_PUBLIC rclcpp::node_interfaces::NodeBaseInterface * diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index 4ba28d5e40..4d9edd0b18 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -60,7 +60,7 @@ class NodeTopicsInterface void add_publisher( rclcpp::PublisherBase::SharedPtr publisher, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; + rclcpp::CallbackGroup::SharedPtr callback_group) = 0; RCLCPP_PUBLIC virtual @@ -75,7 +75,7 @@ class NodeTopicsInterface void add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; + rclcpp::CallbackGroup::SharedPtr callback_group) = 0; RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp index c0b5719a02..fd560bca48 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp @@ -45,14 +45,14 @@ class NodeWaitables : public NodeWaitablesInterface void add_waitable( rclcpp::Waitable::SharedPtr waitable_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) override; + rclcpp::CallbackGroup::SharedPtr group) override; RCLCPP_PUBLIC void remove_waitable( rclcpp::Waitable::SharedPtr waitable_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept override; + rclcpp::CallbackGroup::SharedPtr group) noexcept override; private: RCLCPP_DISABLE_COPY(NodeWaitables) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp index e848cea286..0faae1da62 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp @@ -40,7 +40,7 @@ class NodeWaitablesInterface void add_waitable( rclcpp::Waitable::SharedPtr waitable_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + rclcpp::CallbackGroup::SharedPtr group) = 0; /// \note this function should not throw because it may be called in destructors RCLCPP_PUBLIC @@ -48,7 +48,7 @@ class NodeWaitablesInterface void remove_waitable( rclcpp::Waitable::SharedPtr waitable_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept = 0; + rclcpp::CallbackGroup::SharedPtr group) noexcept = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index c13789598f..32464c556c 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -38,7 +38,7 @@ class NodeOptions /** * Default values for the node options: * - * - context = rclcpp::contexts::default_context::get_global_default_context() + * - context = rclcpp::contexts::get_global_default_context() * - arguments = {} * - parameter_overrides = {} * - use_global_arguments = true @@ -338,7 +338,7 @@ class NodeOptions // documentation in this class. rclcpp::Context::SharedPtr context_ { - rclcpp::contexts::default_context::get_global_default_context()}; + rclcpp::contexts::get_global_default_context()}; std::vector arguments_ {}; diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 73053617dc..021ceb37bd 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -54,21 +54,21 @@ class AsyncParametersClient const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, const std::string & remote_node_name = "", const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr); RCLCPP_PUBLIC AsyncParametersClient( const rclcpp::Node::SharedPtr node, const std::string & remote_node_name = "", const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr); RCLCPP_PUBLIC AsyncParametersClient( rclcpp::Node * node, const std::string & remote_node_name = "", const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr); RCLCPP_PUBLIC std::shared_future> @@ -205,7 +205,7 @@ class SyncParametersClient RCLCPP_PUBLIC SyncParametersClient( - rclcpp::executor::Executor::SharedPtr executor, + rclcpp::Executor::SharedPtr executor, rclcpp::Node::SharedPtr node, const std::string & remote_node_name = "", const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); @@ -218,14 +218,14 @@ class SyncParametersClient RCLCPP_PUBLIC SyncParametersClient( - rclcpp::executor::Executor::SharedPtr executor, + rclcpp::Executor::SharedPtr executor, rclcpp::Node * node, const std::string & remote_node_name = "", const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); RCLCPP_PUBLIC SyncParametersClient( - rclcpp::executor::Executor::SharedPtr executor, + rclcpp::Executor::SharedPtr executor, const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, @@ -339,7 +339,7 @@ class SyncParametersClient } private: - rclcpp::executor::Executor::SharedPtr executor_; + rclcpp::Executor::SharedPtr executor_; const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; AsyncParametersClient::SharedPtr async_parameters_client_; }; diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 75d0e69b9f..dd24718fd7 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -30,10 +30,7 @@ namespace rclcpp { -namespace callback_group -{ class CallbackGroup; -} // namespace callback_group /// Non-templated part of PublisherOptionsWithAllocator. struct PublisherOptionsBase @@ -48,7 +45,7 @@ struct PublisherOptionsBase bool use_default_callbacks = true; /// Callback group in which the waitable items from the publisher should be placed. - std::shared_ptr callback_group; + std::shared_ptr callback_group; /// Optional RMW implementation specific payload to be used during creation of the publisher. std::shared_ptr diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index fc7b4435a4..556085ee07 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -81,7 +81,7 @@ * - rclcpp/executors/multi_threaded_executor.hpp * - CallbackGroups (mechanism for enforcing concurrency rules for callbacks): * - rclcpp::Node::create_callback_group() - * - rclcpp::callback_group::CallbackGroup + * - rclcpp::CallbackGroup * - rclcpp/callback_group.hpp * * Additionally, there are some methods for introspecting the ROS graph: diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index bffd3995a6..4cad8d548b 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -263,7 +263,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy void get_next_subscription( - executor::AnyExecutable & any_exec, + rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override { auto it = subscription_handles_.begin(); @@ -298,7 +298,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy void get_next_service( - executor::AnyExecutable & any_exec, + rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override { auto it = service_handles_.begin(); @@ -332,7 +332,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } void - get_next_client(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override + get_next_client(rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override { auto it = client_handles_.begin(); while (it != client_handles_.end()) { @@ -366,7 +366,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy void get_next_timer( - executor::AnyExecutable & any_exec, + rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override { auto it = timer_handles_.begin(); @@ -400,7 +400,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } void - get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override + get_next_waitable(rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override { 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..0e7d4366e5 100644 --- a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp @@ -17,6 +17,8 @@ #include +#include "rosidl_runtime_cpp/traits.hpp" + #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/visibility_control.hpp" diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index e631ba5da2..12275d74fe 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -45,7 +45,7 @@ struct SubscriptionOptionsBase bool ignore_local_publications = false; /// The callback group for this subscription. NULL to use the default callback group. - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group = nullptr; + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; /// Setting to explicitly set intraprocess communications. IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 690df026bc..d876b2d1d9 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -45,7 +45,7 @@ namespace rclcpp /// Initialize communications via the rmw implementation and set up a global signal handler. /** * Initializes the global context which is accessible via the function - * rclcpp::contexts::default_context::get_global_default_context(). + * rclcpp::contexts::get_global_default_context(). * Also, installs the global signal handlers with the function * rclcpp::install_signal_handlers(). * diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 899d461ed3..1c9b072275 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -74,7 +74,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli const typename StoragePolicy::ServicesIterable & services = {}, const typename StoragePolicy::WaitablesIterable & waitables = {}, rclcpp::Context::SharedPtr context = - rclcpp::contexts::default_context::get_global_default_context()) + rclcpp::contexts::get_global_default_context()) : SynchronizationPolicy(context), StoragePolicy( subscriptions, diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index e02d10701d..769deacb11 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -14,7 +14,7 @@ #include "rclcpp/any_executable.hpp" -using rclcpp::executor::AnyExecutable; +using rclcpp::AnyExecutable; AnyExecutable::AnyExecutable() : subscription(nullptr), diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index e9d0e80c48..db77e46b1e 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -16,8 +16,8 @@ #include -using rclcpp::callback_group::CallbackGroup; -using rclcpp::callback_group::CallbackGroupType; +using rclcpp::CallbackGroup; +using rclcpp::CallbackGroupType; CallbackGroup::CallbackGroup(CallbackGroupType group_type) : type_(group_type), can_be_taken_from_(true) diff --git a/rclcpp/src/rclcpp/contexts/default_context.cpp b/rclcpp/src/rclcpp/contexts/default_context.cpp index 53c6c9f3f2..3ece873db9 100644 --- a/rclcpp/src/rclcpp/contexts/default_context.cpp +++ b/rclcpp/src/rclcpp/contexts/default_context.cpp @@ -14,13 +14,13 @@ #include "rclcpp/contexts/default_context.hpp" -using rclcpp::contexts::default_context::DefaultContext; +using rclcpp::contexts::DefaultContext; DefaultContext::DefaultContext() {} DefaultContext::SharedPtr -rclcpp::contexts::default_context::get_global_default_context() +rclcpp::contexts::get_global_default_context() { static DefaultContext::SharedPtr default_context = DefaultContext::make_shared(); return default_context; diff --git a/rclcpp/src/rclcpp/exceptions.cpp b/rclcpp/src/rclcpp/exceptions/exceptions.cpp similarity index 100% rename from rclcpp/src/rclcpp/exceptions.cpp rename to rclcpp/src/rclcpp/exceptions/exceptions.cpp diff --git a/rclcpp/src/rclcpp/executable_list.cpp b/rclcpp/src/rclcpp/executable_list.cpp index d9dae57cdd..07edbc9586 100644 --- a/rclcpp/src/rclcpp/executable_list.cpp +++ b/rclcpp/src/rclcpp/executable_list.cpp @@ -14,9 +14,9 @@ #include -#include "rclcpp/executable_list.hpp" +#include "rclcpp/experimental/executable_list.hpp" -using rclcpp::executor::ExecutableList; +using rclcpp::experimental::ExecutableList; ExecutableList::ExecutableList() : number_of_subscriptions(0), diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index c25b47db49..2ef663baac 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -29,18 +29,18 @@ #include "rcutils/logging_macros.h" using rclcpp::exceptions::throw_from_rcl_error; -using rclcpp::executor::AnyExecutable; -using rclcpp::executor::Executor; -using rclcpp::executor::ExecutorArgs; -using rclcpp::executor::FutureReturnCode; +using rclcpp::AnyExecutable; +using rclcpp::Executor; +using rclcpp::ExecutorOptions; +using rclcpp::FutureReturnCode; -Executor::Executor(const ExecutorArgs & args) +Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), - memory_strategy_(args.memory_strategy) + memory_strategy_(options.memory_strategy) { rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); rcl_ret_t ret = rcl_guard_condition_init( - &interrupt_guard_condition_, args.context->get_rcl_context().get(), guard_condition_options); + &interrupt_guard_condition_, options.context->get_rcl_context().get(), guard_condition_options); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor"); } @@ -49,14 +49,14 @@ Executor::Executor(const ExecutorArgs & args) // and one for the executor's guard cond (interrupt_guard_condition_) // Put the global ctrl-c guard condition in - memory_strategy_->add_guard_condition(args.context->get_interrupt_guard_condition(&wait_set_)); + memory_strategy_->add_guard_condition(options.context->get_interrupt_guard_condition(&wait_set_)); // Put the executor's guard condition in memory_strategy_->add_guard_condition(&interrupt_guard_condition_); rcl_allocator_t allocator = memory_strategy_->get_allocator(); // Store the context for later use. - context_ = args.context; + context_ = options.context; ret = rcl_wait_set_init( &wait_set_, @@ -502,7 +502,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group) +Executor::get_node_by_group(rclcpp::CallbackGroup::SharedPtr group) { if (!group) { return nullptr; @@ -522,7 +522,7 @@ Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr gro return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) { for (auto & weak_node : weak_nodes_) { @@ -544,7 +544,7 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) } } } - return rclcpp::callback_group::CallbackGroup::SharedPtr(); + return rclcpp::CallbackGroup::SharedPtr(); } bool @@ -625,29 +625,3 @@ 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) -{ - return os << to_string(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 ("; - std::string ret_as_string = std::to_string(static_cast(future_return_code)); - switch (future_return_code) { - case FutureReturnCode::SUCCESS: - prefix = "SUCCESS ("; - break; - case FutureReturnCode::INTERRUPTED: - prefix = "INTERRUPTED ("; - break; - case FutureReturnCode::TIMEOUT: - prefix = "TIMEOUT ("; - break; - } - return prefix + ret_as_string + ")"; -} diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index e2f878abc7..0dfdc35467 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -25,11 +25,11 @@ using rclcpp::executors::MultiThreadedExecutor; MultiThreadedExecutor::MultiThreadedExecutor( - const rclcpp::executor::ExecutorArgs & args, + const rclcpp::ExecutorOptions & options, size_t number_of_threads, bool yield_before_execute, std::chrono::nanoseconds next_exec_timeout) -: executor::Executor(args), +: rclcpp::Executor(options), yield_before_execute_(yield_before_execute), next_exec_timeout_(next_exec_timeout) { @@ -74,7 +74,7 @@ void MultiThreadedExecutor::run(size_t) { while (rclcpp::ok(this->context_) && spinning.load()) { - executor::AnyExecutable any_exec; + rclcpp::AnyExecutable any_exec; { std::lock_guard wait_lock(wait_mutex_); if (!rclcpp::ok(this->context_) || !spinning.load()) { diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 7f699d81ae..360574f7a1 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -18,8 +18,8 @@ using rclcpp::executors::SingleThreadedExecutor; -SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::executor::ExecutorArgs & args) -: executor::Executor(args) {} +SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::ExecutorOptions & options) +: rclcpp::Executor(options) {} SingleThreadedExecutor::~SingleThreadedExecutor() {} @@ -31,7 +31,7 @@ SingleThreadedExecutor::spin() } RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); while (rclcpp::ok(this->context_) && spinning.load()) { - rclcpp::executor::AnyExecutable any_executable; + rclcpp::AnyExecutable any_executable; if (get_next_executable(any_executable)) { execute_any_executable(any_executable); } diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index 0c08e849df..5461e665ab 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -46,7 +46,7 @@ StaticExecutorEntitiesCollector::init( rcl_guard_condition_t * executor_guard_condition) { // Empty initialize executable list - exec_list_ = executor::ExecutableList(); + exec_list_ = rclcpp::experimental::ExecutableList(); // Get executor's wait_set_ pointer p_wait_set_ = p_wait_set; // Get executor's memory strategy ptr diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index c6c1d1be9c..728a0902a1 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -19,11 +19,11 @@ #include "rclcpp/scope_exit.hpp" using rclcpp::executors::StaticSingleThreadedExecutor; -using rclcpp::executor::ExecutableList; +using rclcpp::experimental::ExecutableList; StaticSingleThreadedExecutor::StaticSingleThreadedExecutor( - const rclcpp::executor::ExecutorArgs & args) -: executor::Executor(args) + const rclcpp::ExecutorOptions & options) +: rclcpp::Executor(options) { entities_collector_ = std::make_shared(); } diff --git a/rclcpp/src/rclcpp/future_return_code.cpp b/rclcpp/src/rclcpp/future_return_code.cpp new file mode 100644 index 0000000000..61e167dfb9 --- /dev/null +++ b/rclcpp/src/rclcpp/future_return_code.cpp @@ -0,0 +1,50 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "rclcpp/future_return_code.hpp" + +namespace rclcpp +{ + +std::ostream & +operator<<(std::ostream & os, const rclcpp::FutureReturnCode & future_return_code) +{ + return os << to_string(future_return_code); +} + +std::string +to_string(const rclcpp::FutureReturnCode & future_return_code) +{ + using enum_type = std::underlying_type::type; + std::string prefix = "Unknown enum value ("; + std::string ret_as_string = std::to_string(static_cast(future_return_code)); + switch (future_return_code) { + case FutureReturnCode::SUCCESS: + prefix = "SUCCESS ("; + break; + case FutureReturnCode::INTERRUPTED: + prefix = "INTERRUPTED ("; + break; + case FutureReturnCode::TIMEOUT: + prefix = "TIMEOUT ("; + break; + } + return prefix + ret_as_string + ")"; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index 5a2a5da313..f3973ca9cc 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -127,7 +127,7 @@ MemoryStrategy::get_timer_by_handle( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MemoryStrategy::get_node_by_group( - rclcpp::callback_group::CallbackGroup::SharedPtr group, + rclcpp::CallbackGroup::SharedPtr group, const WeakNodeList & weak_nodes) { if (!group) { @@ -148,7 +148,7 @@ MemoryStrategy::get_node_by_group( return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, const WeakNodeList & weak_nodes) @@ -175,7 +175,7 @@ MemoryStrategy::get_group_by_subscription( return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_service( rclcpp::ServiceBase::SharedPtr service, const WeakNodeList & weak_nodes) @@ -202,7 +202,7 @@ MemoryStrategy::get_group_by_service( return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_client( rclcpp::ClientBase::SharedPtr client, const WeakNodeList & weak_nodes) @@ -229,7 +229,7 @@ MemoryStrategy::get_group_by_client( return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_timer( rclcpp::TimerBase::SharedPtr timer, const WeakNodeList & weak_nodes) @@ -256,7 +256,7 @@ MemoryStrategy::get_group_by_timer( return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_waitable( rclcpp::Waitable::SharedPtr waitable, const WeakNodeList & weak_nodes) diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index b73ec6bf2f..29a25eb645 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -210,15 +210,14 @@ Node::get_logger() const return node_logging_->get_logger(); } -rclcpp::callback_group::CallbackGroup::SharedPtr -Node::create_callback_group( - rclcpp::callback_group::CallbackGroupType group_type) +rclcpp::CallbackGroup::SharedPtr +Node::create_callback_group(rclcpp::CallbackGroupType group_type) { return node_base_->create_callback_group(group_type); } bool -Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +Node::group_in_node(rclcpp::CallbackGroup::SharedPtr group) { return node_base_->callback_group_in_node(group); } @@ -377,7 +376,7 @@ Node::get_subscriptions_info_by_topic(const std::string & topic_name, bool no_ma return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle); } -const std::vector & +const std::vector & Node::get_callback_groups() const { return node_base_->get_callback_groups(); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 6a6f1e4fb1..0bdf2342e5 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -133,7 +133,7 @@ NodeBase::NodeBase( }); // Create the default callback group. - using rclcpp::callback_group::CallbackGroupType; + using rclcpp::CallbackGroupType; default_callback_group_ = create_callback_group(CallbackGroupType::MutuallyExclusive); // Indicate the notify_guard_condition is now valid. @@ -208,24 +208,24 @@ 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::CallbackGroup::SharedPtr +NodeBase::create_callback_group(rclcpp::CallbackGroupType group_type) { - using rclcpp::callback_group::CallbackGroup; - using rclcpp::callback_group::CallbackGroupType; + using rclcpp::CallbackGroup; + using rclcpp::CallbackGroupType; auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type)); callback_groups_.push_back(group); return group; } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr NodeBase::get_default_callback_group() { return default_callback_group_; } bool -NodeBase::callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +NodeBase::callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) { bool group_belongs_to_this_node = false; for (auto & weak_group : this->callback_groups_) { @@ -237,7 +237,7 @@ NodeBase::callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPt return group_belongs_to_this_node; } -const std::vector & +const std::vector & NodeBase::get_callback_groups() const { return callback_groups_; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index 0e74f58907..ebb0de9bfb 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -28,7 +28,7 @@ NodeServices::~NodeServices() void NodeServices::add_service( rclcpp::ServiceBase::SharedPtr service_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { if (group) { if (!node_base_->callback_group_in_node(group)) { @@ -55,7 +55,7 @@ NodeServices::add_service( void NodeServices::add_client( rclcpp::ClientBase::SharedPtr client_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { if (group) { if (!node_base_->callback_group_in_node(group)) { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index f9d5d720e8..d027a0650f 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -28,7 +28,7 @@ NodeTimers::~NodeTimers() void NodeTimers::add_timer( rclcpp::TimerBase::SharedPtr timer, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) + rclcpp::CallbackGroup::SharedPtr callback_group) { if (callback_group) { if (!node_base_->callback_group_in_node(callback_group)) { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 360b4236c3..fced51c3f5 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -44,7 +44,7 @@ NodeTopics::create_publisher( void NodeTopics::add_publisher( rclcpp::PublisherBase::SharedPtr publisher, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) + rclcpp::CallbackGroup::SharedPtr callback_group) { // Assign to a group. if (callback_group) { @@ -83,7 +83,7 @@ NodeTopics::create_subscription( void NodeTopics::add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) + rclcpp::CallbackGroup::SharedPtr callback_group) { // Assign to a group. if (callback_group) { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index ee9a2da2de..0fd083788a 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -28,7 +28,7 @@ NodeWaitables::~NodeWaitables() void NodeWaitables::add_waitable( rclcpp::Waitable::SharedPtr waitable_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { if (group) { if (!node_base_->callback_group_in_node(group)) { @@ -55,7 +55,7 @@ NodeWaitables::add_waitable( void NodeWaitables::remove_waitable( rclcpp::Waitable::SharedPtr waitable_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept + rclcpp::CallbackGroup::SharedPtr group) noexcept { if (group) { if (!node_base_->callback_group_in_node(group)) { diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 3146504005..03a59c6618 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -31,7 +31,7 @@ AsyncParametersClient::AsyncParametersClient( const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, const std::string & remote_node_name, const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) : node_topics_interface_(node_topics_interface) { if (remote_node_name != "") { @@ -103,7 +103,7 @@ AsyncParametersClient::AsyncParametersClient( const rclcpp::Node::SharedPtr node, const std::string & remote_node_name, const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) : AsyncParametersClient( node->get_node_base_interface(), node->get_node_topics_interface(), @@ -118,7 +118,7 @@ AsyncParametersClient::AsyncParametersClient( rclcpp::Node * node, const std::string & remote_node_name, const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) : AsyncParametersClient( node->get_node_base_interface(), node->get_node_topics_interface(), @@ -349,7 +349,7 @@ SyncParametersClient::SyncParametersClient( {} SyncParametersClient::SyncParametersClient( - rclcpp::executor::Executor::SharedPtr executor, + rclcpp::Executor::SharedPtr executor, rclcpp::Node::SharedPtr node, const std::string & remote_node_name, const rmw_qos_profile_t & qos_profile) @@ -375,7 +375,7 @@ SyncParametersClient::SyncParametersClient( {} SyncParametersClient::SyncParametersClient( - rclcpp::executor::Executor::SharedPtr executor, + rclcpp::Executor::SharedPtr executor, rclcpp::Node * node, const std::string & remote_node_name, const rmw_qos_profile_t & qos_profile) @@ -390,7 +390,7 @@ SyncParametersClient::SyncParametersClient( {} SyncParametersClient::SyncParametersClient( - rclcpp::executor::Executor::SharedPtr executor, + rclcpp::Executor::SharedPtr executor, const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, @@ -417,7 +417,7 @@ SyncParametersClient::get_parameters(const std::vector & parameter_ if ( spin_node_until_future_complete( *executor_, node_base_interface_, - f) == rclcpp::executor::FutureReturnCode::SUCCESS) + f) == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } @@ -443,7 +443,7 @@ SyncParametersClient::get_parameter_types(const std::vector & param if ( spin_node_until_future_complete( *executor_, node_base_interface_, - f) == rclcpp::executor::FutureReturnCode::SUCCESS) + f) == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } @@ -460,7 +460,7 @@ SyncParametersClient::set_parameters( if ( spin_node_until_future_complete( *executor_, node_base_interface_, - f) == rclcpp::executor::FutureReturnCode::SUCCESS) + f) == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } @@ -477,7 +477,7 @@ SyncParametersClient::set_parameters_atomically( if ( spin_node_until_future_complete( *executor_, node_base_interface_, - f) == rclcpp::executor::FutureReturnCode::SUCCESS) + f) == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } @@ -496,7 +496,7 @@ SyncParametersClient::list_parameters( if ( spin_node_until_future_complete( *executor_, node_base_interface_, - f) == rclcpp::executor::FutureReturnCode::SUCCESS) + f) == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 6dc8759973..9ece178c28 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -33,7 +33,7 @@ TimerBase::TimerBase( : clock_(clock), timer_handle_(nullptr) { if (nullptr == context) { - context = rclcpp::contexts::default_context::get_global_default_context(); + context = rclcpp::contexts::get_global_default_context(); } auto rcl_context = context->get_rcl_context(); diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 4cbfd27aaa..0c4f0cd705 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -30,7 +30,7 @@ namespace rclcpp void init(int argc, char const * const argv[], const InitOptions & init_options) { - using contexts::default_context::get_global_default_context; + using rclcpp::contexts::get_global_default_context; get_global_default_context()->init(argc, argv, init_options); // Install the signal handlers. install_signal_handlers(); @@ -125,7 +125,7 @@ remove_ros_arguments(int argc, char const * const argv[]) bool ok(Context::SharedPtr context) { - using contexts::default_context::get_global_default_context; + using rclcpp::contexts::get_global_default_context; if (nullptr == context) { context = get_global_default_context(); } @@ -141,7 +141,7 @@ is_initialized(Context::SharedPtr context) bool shutdown(Context::SharedPtr context, const std::string & reason) { - using contexts::default_context::get_global_default_context; + using rclcpp::contexts::get_global_default_context; auto default_context = get_global_default_context(); if (nullptr == context) { context = default_context; @@ -156,7 +156,7 @@ shutdown(Context::SharedPtr context, const std::string & reason) void on_shutdown(std::function callback, Context::SharedPtr context) { - using contexts::default_context::get_global_default_context; + using rclcpp::contexts::get_global_default_context; if (nullptr == context) { context = get_global_default_context(); } @@ -166,7 +166,7 @@ on_shutdown(std::function callback, Context::SharedPtr context) bool sleep_for(const std::chrono::nanoseconds & nanoseconds, Context::SharedPtr context) { - using contexts::default_context::get_global_default_context; + using rclcpp::contexts::get_global_default_context; if (nullptr == context) { context = get_global_default_context(); } diff --git a/rclcpp/test/executors/test_multi_threaded_executor.cpp b/rclcpp/test/executors/test_multi_threaded_executor.cpp index 391c995859..e4d648190f 100644 --- a/rclcpp/test/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/executors/test_multi_threaded_executor.cpp @@ -51,14 +51,14 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { bool yield_before_execute = true; rclcpp::executors::MultiThreadedExecutor executor( - rclcpp::executor::create_default_executor_arguments(), 2u, yield_before_execute); + rclcpp::ExecutorOptions(), 2u, yield_before_execute); ASSERT_GT(executor.get_number_of_threads(), 1u); std::shared_ptr node = std::make_shared("test_multi_threaded_executor_timer_over_take"); - auto cbg = node->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant); + auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); rclcpp::Clock system_clock(RCL_STEADY_TIME); std::mutex last_mutex; diff --git a/rclcpp/test/test_interface_traits.cpp b/rclcpp/test/test_interface_traits.cpp index 599517c328..071ac22ce7 100644 --- a/rclcpp/test/test_interface_traits.cpp +++ b/rclcpp/test/test_interface_traits.cpp @@ -31,7 +31,7 @@ class MyNode return std::make_shared( "my_node_name", "my_node_namespace", - rclcpp::contexts::default_context::get_global_default_context(), + rclcpp::contexts::get_global_default_context(), *options.get_rcl_node_options(), options.use_intra_process_comms(), options.enable_topic_statistics()); diff --git a/rclcpp/test/test_utilities.cpp b/rclcpp/test/test_utilities.cpp index 8dd8d58a0c..d0e05f9bbb 100644 --- a/rclcpp/test/test_utilities.cpp +++ b/rclcpp/test/test_utilities.cpp @@ -52,8 +52,8 @@ TEST(TestUtilities, init_with_args) { } TEST(TestUtilities, multi_init) { - auto context1 = std::make_shared(); - auto context2 = std::make_shared(); + auto context1 = std::make_shared(); + auto context2 = std::make_shared(); EXPECT_FALSE(rclcpp::ok(context1)); EXPECT_FALSE(rclcpp::ok(context2)); diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 6a356ce1aa..2acd4d7aee 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -46,11 +46,11 @@ create_client( rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, const std::string & name, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr) { std::weak_ptr weak_node = node_waitables_interface; - std::weak_ptr weak_group = group; + std::weak_ptr weak_group = group; bool group_is_null = (nullptr == group.get()); auto deleter = [weak_node, weak_group, group_is_null](Client * ptr) @@ -102,9 +102,9 @@ typename Client::SharedPtr create_client( NodeT node, const std::string & name, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr) { - return create_client( + return rclcpp_action::create_client( node->get_node_base_interface(), node->get_node_graph_interface(), node->get_node_logging_interface(), diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index afd24cd285..2886ee9c97 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -65,11 +65,11 @@ create_server( typename Server::CancelCallback handle_cancel, typename Server::AcceptedCallback handle_accepted, const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr) { std::weak_ptr weak_node = node_waitables_interface; - std::weak_ptr weak_group = group; + std::weak_ptr weak_group = group; bool group_is_null = (nullptr == group.get()); auto deleter = [weak_node, weak_group, group_is_null](Server * ptr) @@ -137,7 +137,7 @@ create_server( typename Server::CancelCallback handle_cancel, typename Server::AcceptedCallback handle_accepted, const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr) { return create_server( node->get_node_base_interface(), diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 35e2c4e032..8dd20828cd 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -49,7 +49,7 @@ class TestServer : public ::testing::Test request->goal_id.uuid = uuid; auto future = client->async_send_request(request); if ( - rclcpp::executor::FutureReturnCode::SUCCESS != + rclcpp::FutureReturnCode::SUCCESS != rclcpp::spin_until_future_complete(node, future)) { throw std::runtime_error("send goal future didn't complete succesfully"); @@ -69,7 +69,7 @@ class TestServer : public ::testing::Test request->goal_info.goal_id.uuid = uuid; auto future = cancel_client->async_send_request(request); if ( - rclcpp::executor::FutureReturnCode::SUCCESS != + rclcpp::FutureReturnCode::SUCCESS != rclcpp::spin_until_future_complete(node, future)) { throw std::runtime_error("cancel goal future didn't complete succesfully"); @@ -132,7 +132,7 @@ TEST_F(TestServer, handle_goal_called) auto future = client->async_send_request(request); ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::FutureReturnCode::SUCCESS, rclcpp::spin_until_future_complete(node, future)); ASSERT_EQ(uuid, received_uuid); @@ -744,7 +744,7 @@ TEST_F(TestServer, get_result) // Wait for the result request to be received ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::FutureReturnCode::SUCCESS, rclcpp::spin_until_future_complete(node, future)); auto response = future.get(); diff --git a/rclcpp_components/include/rclcpp_components/component_manager.hpp b/rclcpp_components/include/rclcpp_components/component_manager.hpp index 0003b07e5a..20d2d7b890 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager.hpp @@ -62,7 +62,7 @@ class ComponentManager : public rclcpp::Node RCLCPP_COMPONENTS_PUBLIC ComponentManager( - std::weak_ptr executor, + std::weak_ptr executor, std::string node_name = "ComponentManager", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()); @@ -103,7 +103,7 @@ class ComponentManager : public rclcpp::Node std::shared_ptr response); private: - std::weak_ptr executor_; + std::weak_ptr executor_; uint64_t unique_id_ {1}; std::map> loaders_; diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 720d713031..a80fada378 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -31,7 +31,7 @@ namespace rclcpp_components { ComponentManager::ComponentManager( - std::weak_ptr executor, + std::weak_ptr executor, std::string node_name, const rclcpp::NodeOptions & node_options) : Node(std::move(node_name), node_options), diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index 1d39b04738..a13bf22ec4 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -59,7 +59,7 @@ TEST_F(TestComponentManager, components_api) auto result = client->async_send_request(request); auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. - EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result.get()->success, true); EXPECT_EQ(result.get()->error_message, ""); EXPECT_EQ(result.get()->full_node_name, "/test_component_foo"); @@ -73,7 +73,7 @@ TEST_F(TestComponentManager, components_api) auto result = client->async_send_request(request); auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. - EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result.get()->success, true); EXPECT_EQ(result.get()->error_message, ""); EXPECT_EQ(result.get()->full_node_name, "/test_component_bar"); @@ -89,7 +89,7 @@ TEST_F(TestComponentManager, components_api) auto result = client->async_send_request(request); auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. - EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result.get()->success, true); EXPECT_EQ(result.get()->error_message, ""); EXPECT_EQ(result.get()->full_node_name, "/test_component_baz"); @@ -106,7 +106,7 @@ TEST_F(TestComponentManager, components_api) auto result = client->async_send_request(request); auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. - EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result.get()->success, true); EXPECT_EQ(result.get()->error_message, ""); EXPECT_EQ(result.get()->full_node_name, "/ns/test_component_bing"); @@ -121,7 +121,7 @@ TEST_F(TestComponentManager, components_api) auto result = client->async_send_request(request); auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. - EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result.get()->success, false); EXPECT_EQ(result.get()->error_message, "Failed to find class with the requested plugin name."); EXPECT_EQ(result.get()->full_node_name, ""); @@ -136,7 +136,7 @@ TEST_F(TestComponentManager, components_api) auto result = client->async_send_request(request); auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. - EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result.get()->success, false); EXPECT_EQ(result.get()->error_message, "Could not find requested resource in ament index"); EXPECT_EQ(result.get()->full_node_name, ""); @@ -166,7 +166,7 @@ TEST_F(TestComponentManager, components_api) auto request = std::make_shared(); auto result = client->async_send_request(request); auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. - EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); auto node_names = result.get()->full_node_names; auto unique_ids = result.get()->unique_ids; @@ -197,7 +197,7 @@ TEST_F(TestComponentManager, components_api) auto result = client->async_send_request(request); auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. - EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result.get()->success, true); EXPECT_EQ(result.get()->error_message, ""); } @@ -208,7 +208,7 @@ TEST_F(TestComponentManager, components_api) auto result = client->async_send_request(request); auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. - EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result.get()->success, false); EXPECT_EQ(result.get()->error_message, "No node found with unique_id: 1"); } @@ -226,7 +226,7 @@ TEST_F(TestComponentManager, components_api) auto request = std::make_shared(); auto result = client->async_send_request(request); auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. - EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); auto node_names = result.get()->full_node_names; auto unique_ids = result.get()->unique_ids; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 6b6c213fd5..73d728ba41 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -146,12 +146,12 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, /// Create and return a callback group. RCLCPP_LIFECYCLE_PUBLIC - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + rclcpp::CallbackGroup::SharedPtr + create_callback_group(rclcpp::CallbackGroupType group_type); /// Return the list of callback groups in the node. RCLCPP_LIFECYCLE_PUBLIC - const std::vector & + const std::vector & get_callback_groups() const; /// Create and return a Publisher. @@ -214,7 +214,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Client. */ template @@ -222,7 +222,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, 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); + rclcpp::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Service. */ template @@ -231,7 +231,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Declare and initialize a parameter, return the effective value. /** @@ -659,7 +659,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, RCLCPP_LIFECYCLE_PUBLIC bool - group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group); + group_in_node(rclcpp::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 c09031926a..be79e7944a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -84,7 +84,7 @@ typename rclcpp::WallTimer::SharedPtr LifecycleNode::create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { auto timer = rclcpp::WallTimer::make_shared( std::chrono::duration_cast(period), @@ -98,7 +98,7 @@ 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) + rclcpp::CallbackGroup::SharedPtr group) { rcl_client_options_t options = rcl_client_get_default_options(); options.qos = qos_profile; @@ -123,7 +123,7 @@ LifecycleNode::create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { return rclcpp::create_service( node_base_, node_services_, diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 60debba6f7..7b36f1f753 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -146,9 +146,9 @@ LifecycleNode::get_logger() const return node_logging_->get_logger(); } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr LifecycleNode::create_callback_group( - rclcpp::callback_group::CallbackGroupType group_type) + rclcpp::CallbackGroupType group_type) { return node_base_->create_callback_group(group_type); } @@ -181,7 +181,7 @@ LifecycleNode::set_parameter(const rclcpp::Parameter & parameter) } bool -LifecycleNode::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +LifecycleNode::group_in_node(rclcpp::CallbackGroup::SharedPtr group) { return node_base_->callback_group_in_node(group); } @@ -303,7 +303,7 @@ LifecycleNode::get_subscriptions_info_by_topic(const std::string & topic_name, b return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle); } -const std::vector & +const std::vector & LifecycleNode::get_callback_groups() const { return node_base_->get_callback_groups(); From 2981fb830193a6a22d84ad8633a71d6a08d5229e Mon Sep 17 00:00:00 2001 From: Prajakta Gokhale Date: Thu, 23 Apr 2020 20:41:35 -0700 Subject: [PATCH 027/121] Add received message age metric to topic statistics (#1080) * Add received message age metric to topic statistics Signed-off-by: Prajakta Gokhale * Add unit tests Signed-off-by: Prajakta Gokhale * Add IMU messages in unit test Signed-off-by: Prajakta Gokhale * Use system time instead of steady time Test received message age stats values are greater than 0 Signed-off-by: Devin Bonnie * Fix test warnings Signed-off-by: Devin Bonnie * Replace IMU messages with new dummy messages Signed-off-by: Prajakta Gokhale * Remove outdated TODO and unused test variables Signed-off-by: Prajakta Gokhale * Address review comments Signed-off-by: Devin Bonnie * Address review comments Signed-off-by: Prajakta Gokhale * Re-add message with header for unit testing Signed-off-by: Prajakta Gokhale * Address message review feedback Signed-off-by: Devin Bonnie * Remove extra newline Signed-off-by: Prajakta Gokhale * Address more review feedback Signed-off-by: Devin Bonnie * Fix Windows failure Signed-off-by: Devin Bonnie * Only set append_library_dirs once Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 15 +- rclcpp/include/rclcpp/subscription.hpp | 4 +- .../subscription_topic_statistics.hpp | 8 + rclcpp/package.xml | 1 + rclcpp/test/msg/Header.msg | 3 + rclcpp/test/msg/MessageWithHeader.msg | 3 + .../test_subscription_topic_statistics.cpp | 234 ++++++++++++++---- .../test_topic_stats_utils.hpp | 13 +- 8 files changed, 228 insertions(+), 53 deletions(-) create mode 100644 rclcpp/test/msg/Header.msg create mode 100644 rclcpp/test/msg/MessageWithHeader.msg diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 518951017b..ef2f106c34 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(rmw REQUIRED) find_package(rosgraph_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) find_package(rosidl_runtime_cpp REQUIRED) find_package(rosidl_typesupport_c REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) @@ -223,6 +224,14 @@ if(BUILD_TESTING) add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/test/resources") + rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs + test/msg/Header.msg + test/msg/MessageWithHeader.msg + DEPENDENCIES builtin_interfaces + LIBRARY_NAME ${PROJECT_NAME} + SKIP_INSTALL + ) + ament_add_gtest(test_client test/test_client.cpp) if(TARGET test_client) ament_target_dependencies(test_client @@ -610,9 +619,12 @@ if(BUILD_TESTING) target_link_libraries(test_wait_set ${PROJECT_NAME}) endif() - ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp) + ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + ) if(TARGET test_subscription_topic_statistics) ament_target_dependencies(test_subscription_topic_statistics + "builtin_interfaces" "libstatistics_collector" "rcl_interfaces" "rcutils" @@ -621,6 +633,7 @@ if(BUILD_TESTING) "rosidl_typesupport_cpp" "statistics_msgs" "test_msgs") + rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME}) endif() diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 91343aaee0..2b8108fc94 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -271,8 +271,8 @@ class Subscription : public SubscriptionBase if (subscription_topic_statistics_) { const auto nanos = std::chrono::time_point_cast( - std::chrono::steady_clock::now()); - const auto time = rclcpp::Time(nanos.time_since_epoch().count(), RCL_STEADY_TIME); + std::chrono::system_clock::now()); + const auto time = rclcpp::Time(nanos.time_since_epoch().count()); subscription_topic_statistics_->handle_message(*typed_message, time); } } diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index 058ed45818..37a2804cbb 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -23,6 +23,7 @@ #include "libstatistics_collector/collector/generate_statistics_message.hpp" #include "libstatistics_collector/moving_average_statistics/types.hpp" #include "libstatistics_collector/topic_statistics_collector/constants.hpp" +#include "libstatistics_collector/topic_statistics_collector/received_message_age.hpp" #include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp" #include "rcl/time.h" @@ -56,6 +57,9 @@ class SubscriptionTopicStatistics using TopicStatsCollector = libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector< CallbackMessageT>; + using ReceivedMessageAge = + libstatistics_collector::topic_statistics_collector::ReceivedMessageAgeCollector< + CallbackMessageT>; using ReceivedMessagePeriod = libstatistics_collector::topic_statistics_collector::ReceivedMessagePeriodCollector< CallbackMessageT>; @@ -173,6 +177,10 @@ class SubscriptionTopicStatistics */ void bring_up() { + auto received_message_age = std::make_unique(); + received_message_age->Start(); + subscriber_statistics_collectors_.emplace_back(std::move(received_message_age)); + auto received_message_period = std::make_unique(); received_message_period->Start(); { diff --git a/rclcpp/package.xml b/rclcpp/package.xml index a5e09c0f18..4e7f15b511 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -37,6 +37,7 @@ ament_lint_common rmw rmw_implementation_cmake + rosidl_default_generators test_msgs diff --git a/rclcpp/test/msg/Header.msg b/rclcpp/test/msg/Header.msg new file mode 100644 index 0000000000..bfe5dc5366 --- /dev/null +++ b/rclcpp/test/msg/Header.msg @@ -0,0 +1,3 @@ +# Simple Header message with a timestamp field. + +builtin_interfaces/Time stamp diff --git a/rclcpp/test/msg/MessageWithHeader.msg b/rclcpp/test/msg/MessageWithHeader.msg new file mode 100644 index 0000000000..3bd1b45534 --- /dev/null +++ b/rclcpp/test/msg/MessageWithHeader.msg @@ -0,0 +1,3 @@ +# Message containing a simple Header field. + +Header header diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 37433f0f2f..3a7e90c025 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -18,12 +18,14 @@ #include #include #include +#include #include #include #include "libstatistics_collector/moving_average_statistics/types.hpp" #include "rclcpp/create_publisher.hpp" +#include "rclcpp/msg/message_with_header.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/rclcpp.hpp" @@ -49,6 +51,7 @@ constexpr const uint64_t kNoSamples{0}; constexpr const std::chrono::seconds kTestDuration{10}; } // namespace +using rclcpp::msg::MessageWithHeader; using test_msgs::msg::Empty; using rclcpp::topic_statistics::SubscriptionTopicStatistics; using statistics_msgs::msg::MetricsMessage; @@ -96,21 +99,47 @@ class EmptyPublisher : public rclcpp::Node virtual ~EmptyPublisher() = default; - size_t get_number_published() +private: + void publish_message() { - return number_published_.load(); + auto msg = Empty{}; + publisher_->publish(msg); + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr publish_timer_; +}; + +/** + * MessageWithHeader publisher node: used to publish MessageWithHeader with `header` value set + */ +class MessageWithHeaderPublisher : public rclcpp::Node +{ +public: + MessageWithHeaderPublisher( + const std::string & name, const std::string & topic, + const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) + : Node(name) + { + publisher_ = create_publisher(topic, 10); + publish_timer_ = this->create_wall_timer( + publish_period, [this]() { + this->publish_message(); + }); } + virtual ~MessageWithHeaderPublisher() = default; + private: void publish_message() { - ++number_published_; - auto msg = Empty{}; + auto msg = MessageWithHeader{}; + // Subtract 1 sec from current time so the received message age is always > 0 + msg.header.stamp = this->now() - rclcpp::Duration{1, 0}; publisher_->publish(msg); } - rclcpp::Publisher::SharedPtr publisher_; - std::atomic number_published_{0}; + rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr publish_timer_; }; @@ -127,8 +156,8 @@ class EmptySubscriber : public rclcpp::Node auto options = rclcpp::SubscriptionOptions(); options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; - auto callback = [this](Empty::UniquePtr msg) { - this->receive_message(*msg); + auto callback = [](Empty::UniquePtr msg) { + (void) msg; }; subscription_ = create_subscription>( @@ -140,10 +169,36 @@ class EmptySubscriber : public rclcpp::Node virtual ~EmptySubscriber() = default; private: - void receive_message(const Empty &) const + rclcpp::Subscription::SharedPtr subscription_; +}; + +/** + * MessageWithHeader subscriber node: used to create subscriber topic statistics requirements + */ +class MessageWithHeaderSubscriber : public rclcpp::Node +{ +public: + MessageWithHeaderSubscriber(const std::string & name, const std::string & topic) + : Node(name) { + // manually enable topic statistics via options + auto options = rclcpp::SubscriptionOptions(); + options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; + + auto callback = [](MessageWithHeader::UniquePtr msg) { + (void) msg; + }; + subscription_ = create_subscription>( + topic, + rclcpp::QoS(rclcpp::KeepAll()), + callback, + options); } - rclcpp::Subscription::SharedPtr subscription_; + virtual ~MessageWithHeaderSubscriber() = default; + +private: + rclcpp::Subscription::SharedPtr subscription_; }; /** @@ -170,18 +225,18 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) { - // manually create publisher tied to the node + // Manually create publisher tied to the node auto topic_stats_publisher = empty_subscriber->create_publisher( kTestTopicStatisticsTopic, 10); - // construct a separate instance + // Construct a separate instance auto sub_topic_stats = std::make_unique>( empty_subscriber->get_name(), topic_stats_publisher); - // expect no data has been collected / no samples received + // Expect no data has been collected / no samples received for (const auto & data : sub_topic_stats->get_current_collector_data()) { EXPECT_TRUE(std::isnan(data.average)); EXPECT_TRUE(std::isnan(data.min)); @@ -191,56 +246,147 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) } } -TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_single_empty_stats_message) +TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no_header) { - // create an empty publisher + // Create an empty publisher auto empty_publisher = std::make_shared( kTestPubNodeName, kTestSubStatsTopic); // empty_subscriber has a topic statistics instance as part of its subscription // this will listen to and generate statistics for the empty message - // create a listener for topic statistics messages + // Create a listener for topic statistics messages auto statistics_listener = std::make_shared( "test_receive_single_empty_stats_message_listener", - "/statistics"); + "/statistics", + 2); rclcpp::executors::SingleThreadedExecutor ex; ex.add_node(empty_publisher); ex.add_node(statistics_listener); ex.add_node(empty_subscriber); - // spin and get future + // Spin and get future ex.spin_until_future_complete( statistics_listener->GetFuture(), kTestDuration); - // compare message counts, sample count should be the same as published and received count - EXPECT_EQ(1, statistics_listener->GetNumberOfMessagesReceived()); - - // check the received message and the data types - const auto received_message = statistics_listener->GetLastReceivedMessage(); - for (const auto & stats_point : received_message.statistics) { - const auto type = stats_point.data_type; - switch (type) { - case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: - EXPECT_LT(0, stats_point.data) << "unexpected sample count"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: - EXPECT_LT(0, stats_point.data) << "unexpected avg"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: - EXPECT_LT(0, stats_point.data) << "unexpected min"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: - EXPECT_LT(0, stats_point.data) << "unexpected max"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: - EXPECT_LT(0, stats_point.data) << "unexpected stddev"; - break; - default: - FAIL() << "received unknown statistics type: " << std::dec << - static_cast(type); + // Compare message counts, sample count should be the same as published and received count + EXPECT_EQ(2, statistics_listener->GetNumberOfMessagesReceived()); + + // Check the received message and the data types + const auto received_messages = statistics_listener->GetReceivedMessages(); + + EXPECT_EQ(2u, received_messages.size()); + + std::set received_metrics; + for (const auto & msg : received_messages) { + received_metrics.insert(msg.metrics_source); + } + EXPECT_TRUE(received_metrics.find("message_age") != received_metrics.end()); + EXPECT_TRUE(received_metrics.find("message_period") != received_metrics.end()); + + // Check the collected statistics for message period. + // Message age statistics will not be calculated because Empty messages + // don't have a `header` with timestamp. + for (const auto & msg : received_messages) { + if (msg.metrics_source != "message_period") { + continue; + } + for (const auto & stats_point : msg.statistics) { + const auto type = stats_point.data_type; + switch (type) { + case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: + EXPECT_LT(0, stats_point.data) << "unexpected sample count"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: + EXPECT_LT(0, stats_point.data) << "unexpected avg"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected min"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected max"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: + EXPECT_LT(0, stats_point.data) << "unexpected stddev"; + break; + default: + FAIL() << "received unknown statistics type: " << std::dec << + static_cast(type); + } + } + } +} + +TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_with_header) +{ + // Create a MessageWithHeader publisher + auto msg_with_header_publisher = std::make_shared( + kTestPubNodeName, + kTestSubStatsTopic); + // empty_subscriber has a topic statistics instance as part of its subscription + // this will listen to and generate statistics for the empty message + + // Create a listener for topic statistics messages + auto statistics_listener = std::make_shared( + "test_receive_stats_for_message_with_header", + "/statistics", + 2); + + auto msg_with_header_subscriber = std::make_shared( + kTestSubNodeName, + kTestSubStatsTopic); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(msg_with_header_publisher); + ex.add_node(statistics_listener); + ex.add_node(msg_with_header_subscriber); + + // Spin and get future + ex.spin_until_future_complete( + statistics_listener->GetFuture(), + kTestDuration); + + // Compare message counts, sample count should be the same as published and received count + EXPECT_EQ(2, statistics_listener->GetNumberOfMessagesReceived()); + + // Check the received message and the data types + const auto received_messages = statistics_listener->GetReceivedMessages(); + + EXPECT_EQ(2u, received_messages.size()); + + std::set received_metrics; + for (const auto & msg : received_messages) { + received_metrics.insert(msg.metrics_source); + } + EXPECT_TRUE(received_metrics.find("message_age") != received_metrics.end()); + EXPECT_TRUE(received_metrics.find("message_period") != received_metrics.end()); + + // Check the collected statistics for message period. + for (const auto & msg : received_messages) { + for (const auto & stats_point : msg.statistics) { + const auto type = stats_point.data_type; + switch (type) { + case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: + EXPECT_LT(0, stats_point.data) << "unexpected sample count"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: + EXPECT_LT(0, stats_point.data) << "unexpected avg"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected min"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected max"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: + EXPECT_LE(0, stats_point.data) << "unexpected stddev"; + break; + default: + FAIL() << "received unknown statistics type: " << std::dec << + static_cast(type); + } } } } diff --git a/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp b/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp index 0140f919e8..1399abbaff 100644 --- a/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp +++ b/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "statistics_msgs/msg/metrics_message.hpp" @@ -89,7 +90,7 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter MetricsMessageSubscriber( const std::string & name, const std::string & topic_name, - const int number_of_messages_to_receive = 1) + const int number_of_messages_to_receive = 2) : rclcpp::Node(name), number_of_messages_to_receive_(number_of_messages_to_receive) { @@ -99,7 +100,7 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter subscription_ = create_subscription>( topic_name, - 0 /*history_depth*/, + 10 /*history_depth*/, callback); } @@ -107,10 +108,10 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter * Acquires a mutex in order to get the last message received member. * \return the last message received */ - MetricsMessage GetLastReceivedMessage() const + std::vector GetReceivedMessages() const { std::unique_lock ulock{mutex_}; - return last_received_message_; + return received_messages_; } /** @@ -132,13 +133,13 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter { std::unique_lock ulock{mutex_}; ++num_messages_received_; - last_received_message_ = msg; + received_messages_.push_back(msg); if (num_messages_received_ >= number_of_messages_to_receive_) { PromiseSetter::SetPromise(); } } - MetricsMessage last_received_message_; + std::vector received_messages_; rclcpp::Subscription::SharedPtr subscription_; mutable std::mutex mutex_; std::atomic num_messages_received_{0}; From 5793c07973e43f25ce932202d802410dcea62af8 Mon Sep 17 00:00:00 2001 From: brawner Date: Sun, 26 Apr 2020 21:40:40 -0700 Subject: [PATCH 028/121] Add serialized_message.hpp header (#1095) Signed-off-by: Stephen Brawner --- rclcpp/include/rclcpp/message_memory_strategy.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index 6e4c0be065..f548d953c2 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -23,6 +23,7 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/serialized_message.hpp" #include "rclcpp/visibility_control.hpp" #include "rcutils/logging_macros.h" From 8f34d88759cbd172ada664495597957311fe484a Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Mon, 27 Apr 2020 10:30:33 -0700 Subject: [PATCH 029/121] export targets in a addition to include directories / libraries (#1096) Signed-off-by: Dirk Thomas --- rclcpp_action/CMakeLists.txt | 9 +++++++-- rclcpp_lifecycle/CMakeLists.txt | 9 ++++++--- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 25bc985426..48555ed81f 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -16,8 +16,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -include_directories(include) - set(${PROJECT_NAME}_SRCS src/client.cpp src/qos.cpp @@ -29,6 +27,11 @@ set(${PROJECT_NAME}_SRCS add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SRCS}) +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$") + ament_target_dependencies(${PROJECT_NAME} "action_msgs" "rcl_action" @@ -47,6 +50,7 @@ install( install( TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -55,6 +59,7 @@ install( # specific order: dependents before dependencies ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(${PROJECT_NAME}) ament_export_dependencies(ament_cmake) ament_export_dependencies(action_msgs) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index d7a195f2fa..ab4cf4f857 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -16,8 +16,6 @@ find_package(rcl_lifecycle REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) find_package(lifecycle_msgs REQUIRED) -include_directories(include) - ### CPP High level library add_library(rclcpp_lifecycle src/lifecycle_node.cpp @@ -25,6 +23,10 @@ add_library(rclcpp_lifecycle src/state.cpp src/transition.cpp ) +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$") # specific order: dependents before dependencies ament_target_dependencies(rclcpp_lifecycle "rclcpp" @@ -38,7 +40,7 @@ ament_target_dependencies(rclcpp_lifecycle target_compile_definitions(rclcpp_lifecycle PRIVATE "RCLCPP_LIFECYCLE_BUILDING_DLL") install(TARGETS - rclcpp_lifecycle + rclcpp_lifecycle EXPORT rclcpp_lifecycle ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) @@ -102,6 +104,7 @@ endif() # specific order: dependents before dependencies ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(${PROJECT_NAME}) ament_export_dependencies(rclcpp) ament_export_dependencies(rcl_lifecycle) ament_export_dependencies(lifecycle_msgs) From 64a2f146b5ed95f783d210a9ef4c1f95f67d6052 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 27 Apr 2020 15:14:48 -0300 Subject: [PATCH 030/121] Enforce a precedence for wildcard matching in parameter overrides. (#1094) Signed-off-by: Michel Hidalgo --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 11 +++++++---- rclcpp/test/resources/test_node/test_parameters.yaml | 6 +++++- rclcpp/test/test_node.cpp | 5 +++-- 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 40db54b9c5..f5d0aa7e64 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -107,11 +107,14 @@ NodeParameters::NodeParameters( rcl_yaml_node_struct_fini(params); }); rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params); - for (auto iter = initial_map.begin(); initial_map.end() != iter; iter++) { - // TODO(cottsay) implement further wildcard matching - if (iter->first == "/**" || iter->first == combined_name_) { + + // Enforce wildcard matching precedence + // TODO(cottsay) implement further wildcard matching + const std::vector node_matching_names{"/**", combined_name_}; + for (const auto & node_name : node_matching_names) { + if (initial_map.count(node_name) > 0) { // Combine parameter yaml files, overwriting values in older ones - for (auto & param : iter->second) { + for (const rclcpp::Parameter & param : initial_map.at(node_name)) { parameter_overrides_[param.get_name()] = rclcpp::ParameterValue(param.get_value_message()); } diff --git a/rclcpp/test/resources/test_node/test_parameters.yaml b/rclcpp/test/resources/test_node/test_parameters.yaml index 2c4e30789a..449caa36bc 100644 --- a/rclcpp/test/resources/test_node/test_parameters.yaml +++ b/rclcpp/test/resources/test_node/test_parameters.yaml @@ -1,4 +1,8 @@ /**: ros__parameters: - parameter_int: 21 + parameter_bool: true + parameter_int: 42 parameter_string_array: [baz, baz, baz] +test_declare_parameter_node: + ros__parameters: + parameter_int: 21 diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp index c85954aedf..e4851e17d8 100644 --- a/rclcpp/test/test_node.cpp +++ b/rclcpp/test/test_node.cpp @@ -691,7 +691,7 @@ TEST_F(TestNode, declare_parameter_with_cli_overrides) { no.arguments( { "--ros-args", - "-p", "parameter_bool:=true", + "-p", "parameter_bool:=false", "-p", "parameter_int:=42", "-p", "parameter_double:=0.42", "-p", "parameter_string:=foo", @@ -702,7 +702,8 @@ TEST_F(TestNode, declare_parameter_with_cli_overrides) { "-p", "parameter_string_array:=[foo, bar]" }); - auto node = std::make_shared("test_declare_parameter_node"_unq, no); + // To match parameters YAML file content, use a well-known node name for this test only. + auto node = std::make_shared("test_declare_parameter_node", no); { rclcpp::ParameterValue value = node->declare_parameter("parameter_bool"); EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_BOOL); From 4e4a17c38c97640d2761d0889544376d12b45a56 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 27 Apr 2020 22:23:43 -0700 Subject: [PATCH 031/121] Serialized message move constructor (#1097) * correct use of move semantics Signed-off-by: Knese Karsten * more tests Signed-off-by: Knese Karsten * make error message more exact Signed-off-by: Karsten Knese * use std::exchange Signed-off-by: Karsten Knese * fix typo Signed-off-by: Karsten Knese --- rclcpp/src/rclcpp/serialization.cpp | 7 ++-- rclcpp/src/rclcpp/serialized_message.cpp | 43 ++++++++++++------------ rclcpp/test/test_serialized_message.cpp | 37 +++++++++++++++++--- 3 files changed, 60 insertions(+), 27 deletions(-) diff --git a/rclcpp/src/rclcpp/serialization.cpp b/rclcpp/src/rclcpp/serialization.cpp index 689757c98c..bca341185f 100644 --- a/rclcpp/src/rclcpp/serialization.cpp +++ b/rclcpp/src/rclcpp/serialization.cpp @@ -55,8 +55,11 @@ void SerializationBase::deserialize_message( rcpputils::check_true(nullptr != type_support_, "Typesupport is nullpointer."); rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer."); rcpputils::check_true( - 0 != serialized_message->capacity() && 0 != serialized_message->size(), - "Serialized message is wrongly initialized."); + 0u != serialized_message->capacity(), + "Wrongly initialized. Serialized message has a capacity of zero."); + rcpputils::check_true( + 0u != serialized_message->size(), + "Wrongly initialized. Serialized message has a size of zero."); rcpputils::check_true(nullptr != ros_message, "ROS message is a nullpointer."); const auto ret = rmw_deserialize( diff --git a/rclcpp/src/rclcpp/serialized_message.cpp b/rclcpp/src/rclcpp/serialized_message.cpp index 64e54f58f9..2caaf90edb 100644 --- a/rclcpp/src/rclcpp/serialized_message.cpp +++ b/rclcpp/src/rclcpp/serialized_message.cpp @@ -66,50 +66,51 @@ SerializedMessage::SerializedMessage(const rcl_serialized_message_t & other) } SerializedMessage::SerializedMessage(SerializedMessage && other) -: SerializedMessage(other.serialized_message_) -{ - other.serialized_message_ = rmw_get_zero_initialized_serialized_message(); -} +: serialized_message_( + std::exchange(other.serialized_message_, rmw_get_zero_initialized_serialized_message())) +{} SerializedMessage::SerializedMessage(rcl_serialized_message_t && other) -: serialized_message_(other) -{ - // reset buffer to prevent double free - other = rmw_get_zero_initialized_serialized_message(); -} +: serialized_message_( + std::exchange(other, rmw_get_zero_initialized_serialized_message())) +{} SerializedMessage & SerializedMessage::operator=(const SerializedMessage & other) { - serialized_message_ = rmw_get_zero_initialized_serialized_message(); - copy_rcl_message(other.serialized_message_, serialized_message_); + if (this != &other) { + serialized_message_ = rmw_get_zero_initialized_serialized_message(); + copy_rcl_message(other.serialized_message_, serialized_message_); + } return *this; } SerializedMessage & SerializedMessage::operator=(const rcl_serialized_message_t & other) { - serialized_message_ = rmw_get_zero_initialized_serialized_message(); - copy_rcl_message(other, serialized_message_); + if (&serialized_message_ != &other) { + serialized_message_ = rmw_get_zero_initialized_serialized_message(); + copy_rcl_message(other, serialized_message_); + } return *this; } SerializedMessage & SerializedMessage::operator=(SerializedMessage && other) { - *this = other.serialized_message_; - other.serialized_message_ = rmw_get_zero_initialized_serialized_message(); + if (this != &other) { + serialized_message_ = + std::exchange(other.serialized_message_, rmw_get_zero_initialized_serialized_message()); + } return *this; } SerializedMessage & SerializedMessage::operator=(rcl_serialized_message_t && other) { - serialized_message_ = rmw_get_zero_initialized_serialized_message(); - serialized_message_ = other; - - // reset original to prevent double free - other = rmw_get_zero_initialized_serialized_message(); - + if (&serialized_message_ != &other) { + serialized_message_ = + std::exchange(other, rmw_get_zero_initialized_serialized_message()); + } return *this; } diff --git a/rclcpp/test/test_serialized_message.cpp b/rclcpp/test/test_serialized_message.cpp index 9047e22b22..4e28106a88 100644 --- a/rclcpp/test/test_serialized_message.cpp +++ b/rclcpp/test/test_serialized_message.cpp @@ -52,6 +52,7 @@ TEST(TestSerializedMessage, various_constructors) { rcl_handle.buffer_length = content_size; EXPECT_STREQ(content.c_str(), reinterpret_cast(rcl_handle.buffer)); EXPECT_EQ(content_size, serialized_message.capacity()); + EXPECT_EQ(content_size, serialized_message.size()); // Copy Constructor rclcpp::SerializedMessage other_serialized_message(serialized_message); @@ -158,9 +159,9 @@ TEST(TestSerializedMessage, serialization) { } } -template -void test_empty_msg_serialize() +void serialize_default_ros_msg() { + using MessageT = test_msgs::msg::BasicTypes; rclcpp::Serialization serializer; MessageT ros_msg; rclcpp::SerializedMessage serialized_msg; @@ -168,12 +169,40 @@ void test_empty_msg_serialize() serializer.serialize_message(&ros_msg, &serialized_msg); } -template -void test_empty_msg_deserialize() +void serialize_default_ros_msg_into_nullptr() +{ + using MessageT = test_msgs::msg::BasicTypes; + rclcpp::Serialization serializer; + MessageT ros_msg; + + serializer.serialize_message(&ros_msg, nullptr); +} + +void deserialize_default_serialized_message() +{ + using MessageT = test_msgs::msg::BasicTypes; + rclcpp::Serialization serializer; + MessageT ros_msg; + rclcpp::SerializedMessage serialized_msg; + + serializer.deserialize_message(&serialized_msg, &ros_msg); +} + +void deserialize_nullptr() { + using MessageT = test_msgs::msg::BasicTypes; rclcpp::Serialization serializer; MessageT ros_msg; rclcpp::SerializedMessage serialized_msg; serializer.deserialize_message(&serialized_msg, &ros_msg); } + +TEST(TestSerializedMessage, serialization_empty_messages) +{ + EXPECT_NO_THROW(serialize_default_ros_msg()); + EXPECT_THROW(serialize_default_ros_msg_into_nullptr(), rcpputils::IllegalStateException); + EXPECT_THROW(serialize_default_ros_msg_into_nullptr(), rcpputils::IllegalStateException); + EXPECT_THROW(deserialize_default_serialized_message(), rcpputils::IllegalStateException); + EXPECT_THROW(deserialize_nullptr(), rcpputils::IllegalStateException); +} From a4a97dea9812f43a0c93e849690e55b085321f79 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 29 Apr 2020 08:45:04 +0200 Subject: [PATCH 032/121] Added rclcpp_components Doxyfile (#1091) * Added rclcpp components Doxyfile Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde --- rclcpp_components/Doxyfile | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 rclcpp_components/Doxyfile diff --git a/rclcpp_components/Doxyfile b/rclcpp_components/Doxyfile new file mode 100644 index 0000000000..da33050c4a --- /dev/null +++ b/rclcpp_components/Doxyfile @@ -0,0 +1,33 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rclcpp_components" +PROJECT_NUMBER = master +PROJECT_BRIEF = "Package containing tools for dynamically loadable components" + +# Use these lines to include the generated logging.hpp (update install path if needed) +# Otherwise just generate for the local (non-generated header files) +INPUT = ./include + +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED = RCLCPP_COMPONENTS_PUBLIC= + +# Tag files that do not exist will produce a warning and cross-project linking will not work. +TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) +TAGFILES += "../../../../doxygen_tag_files/class_loader.tag=http://docs.ros2.org/latest/api/class_loader/" +TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" +TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +# Uncomment to generate tag files for cross-project linking. +GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp_components.tag" From 1521181550486372c03fc58ed2ea881cd48361a5 Mon Sep 17 00:00:00 2001 From: brawner Date: Wed, 29 Apr 2020 14:25:52 -0700 Subject: [PATCH 033/121] Increasing test coverage of rclcpp_action (#1043) * Increasing test coverage of rclcpp_action Signed-off-by: Stephen Brawner * PR Fixup Signed-off-by: Stephen Brawner * PR Fixup Signed-off-by: Stephen Brawner * PR Fixup Signed-off-by: Stephen Brawner * Increasing test coverage of rclcpp_action Signed-off-by: Stephen Brawner * PR Fixup Signed-off-by: Stephen Brawner * PR Fixup Signed-off-by: Stephen Brawner * PR Fixup Signed-off-by: Stephen Brawner * Fix warnings Signed-off-by: Stephen Brawner --- rclcpp_action/CMakeLists.txt | 10 +++++ rclcpp_action/test/test_client.cpp | 15 ++++++++ rclcpp_action/test/test_server.cpp | 26 +++++++++++++ rclcpp_action/test/test_types.cpp | 61 ++++++++++++++++++++++++++++++ 4 files changed, 112 insertions(+) create mode 100644 rclcpp_action/test/test_types.cpp diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 48555ed81f..598f37a50f 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -103,6 +103,16 @@ if(BUILD_TESTING) ${PROJECT_NAME} ) endif() + + ament_add_gtest(test_types test/test_types.cpp) + if(TARGET test_types) + ament_target_dependencies(test_types + "test_msgs" + ) + target_link_libraries(test_types + ${PROJECT_NAME} + ) + endif() endif() ament_package() diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 82a6dbda60..e89b19a470 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -270,6 +270,21 @@ TEST_F(TestClient, construction_and_destruction) ASSERT_NO_THROW(rclcpp_action::create_client(client_node, action_name).reset()); } +TEST_F(TestClient, construction_and_destruction_callback_group) +{ + auto group = client_node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + ASSERT_NO_THROW( + rclcpp_action::create_client( + client_node->get_node_base_interface(), + client_node->get_node_graph_interface(), + client_node->get_node_logging_interface(), + client_node->get_node_waitables_interface(), + action_name, + group + ).reset()); +} + TEST_F(TestClient, async_send_goal_no_callbacks) { auto action_client = rclcpp_action::create_client(client_node, action_name); diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 8dd20828cd..7c873f5ee1 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -95,6 +95,32 @@ TEST_F(TestServer, construction_and_destruction) (void)as; } +TEST_F(TestServer, construction_and_destruction_callback_group) +{ + auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); + auto group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(); + + using GoalHandle = rclcpp_action::ServerGoalHandle; + ASSERT_NO_THROW( + rclcpp_action::create_server( + node->get_node_base_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}, + options, + group)); +} + TEST_F(TestServer, handle_goal_called) { auto node = std::make_shared("handle_goal_node", "/rclcpp_action/handle_goal"); diff --git a/rclcpp_action/test/test_types.cpp b/rclcpp_action/test/test_types.cpp new file mode 100644 index 0000000000..f981261105 --- /dev/null +++ b/rclcpp_action/test/test_types.cpp @@ -0,0 +1,61 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include "rclcpp_action/types.hpp" + +TEST(TestActionTypes, goal_uuid_to_string) { + rclcpp_action::GoalUUID goal_id; + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = i; + } + EXPECT_STREQ("0123456789abcdef", rclcpp_action::to_string(goal_id).c_str()); + + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = 16u + i; + } + EXPECT_STREQ("101112131415161718191a1b1c1d1e1f", rclcpp_action::to_string(goal_id).c_str()); + + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = std::numeric_limits::max() - i; + } + EXPECT_STREQ("fffefdfcfbfaf9f8f7f6f5f4f3f2f1f0", rclcpp_action::to_string(goal_id).c_str()); +} + +TEST(TestActionTypes, goal_uuid_to_rcl_action_goal_info) { + rclcpp_action::GoalUUID goal_id; + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = i; + } + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + rclcpp_action::convert(goal_id, &goal_info); + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + EXPECT_EQ(goal_info.goal_id.uuid[i], goal_id[i]); + } +} + +TEST(TestActionTypes, rcl_action_goal_info_to_goal_uuid) { + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_info.goal_id.uuid[i] = i; + } + + rclcpp_action::GoalUUID goal_id; + rclcpp_action::convert(goal_id, &goal_info); + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + EXPECT_EQ(goal_info.goal_id.uuid[i], goal_id[i]); + } +} From 8f96a038cb346e274ec93bf92f41203588e4ac74 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 29 Apr 2020 22:44:16 -0700 Subject: [PATCH 034/121] 0.9.0 --- rclcpp/CHANGELOG.rst | 83 +++++++++++++++++++++++++++++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 12 +++++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 12 +++++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 13 ++++++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 124 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index e03896e7fb..f3ee617c58 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,89 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.9.0 (2020-04-29) +------------------ +* Serialized message move constructor (`#1097 `_) +* Enforce a precedence for wildcard matching in parameter overrides. (`#1094 `_) +* Add serialized_message.hpp header (`#1095 `_) +* Add received message age metric to topic statistics (`#1080 `_) +* Deprecate redundant namespaces (`#1083 `_) +* Export targets in addition to include directories / libraries (`#1088 `_) +* Ensure logging is initialized just once (`#998 `_) +* Adapt subscription traits to rclcpp::SerializedMessage (`#1092 `_) +* Protect subscriber_statistics_collectors\_ with a mutex (`#1084 `_) +* Remove unused test variable (`#1087 `_) +* Use serialized message (`#1081 `_) +* Integrate topic statistics (`#1072 `_) +* Fix rclcpp interface traits test (`#1086 `_) +* Generate node interfaces' getters and traits (`#1069 `_) +* Use composition for serialized message (`#1082 `_) +* Dnae adas/serialized message (`#1075 `_) +* Reflect changes in rclcpp API (`#1079 `_) +* Fix build regression (`#1078 `_) +* Add NodeDefault option for enabling topic statistics (`#1074 `_) +* Topic Statistics: Add SubscriptionTopicStatistics class (`#1050 `_) +* Add SubscriptionOptions for topic statistics (`#1057 `_) +* Remove warning message from failing to register default callback (`#1067 `_) +* Create a default warning for qos incompatibility (`#1051 `_) +* Add WaitSet class and modify entities to work without executor (`#1047 `_) +* Include what you use (`#1059 `_) +* Rename rosidl_generator_cpp namespace to rosidl_runtime_cpp (`#1060 `_) +* Changed rosidl_generator_c/cpp to rosidl_runtime_c/cpp (`#1014 `_) +* Use constexpr for endpoint type name (`#1055 `_) +* Add InvalidParameterTypeException (`#1027 `_) +* Support for ON_REQUESTED_INCOMPATIBLE_QOS and ON_OFFERED_INCOMPATIBLE_QOS events (`#924 `_) +* Fixup clang warning (`#1040 `_) +* Adding a "static" single threaded executor (`#1034 `_) +* Add equality operators for QoS profile (`#1032 `_) +* Remove extra vertical whitespace (`#1030 `_) +* Switch IntraProcessMessage to test_msgs/Empty (`#1017 `_) +* Add new type of exception that may be thrown during creation of publisher/subscription (`#1026 `_) +* Don't check lifespan on publisher QoS (`#1002 `_) +* Fix get_parameter_tyeps of AsyncPrameterClient results are always empty (`#1019 `_) +* Cleanup node interfaces includes (`#1016 `_) +* Add ifdefs to remove tracing-related calls if tracing is disabled (`#1001 `_) +* Include missing header in node_graph.cpp (`#994 `_) +* Add missing includes of logging.hpp (`#995 `_) +* Zero initialize publisher GID in subscription intra process callback (`#1011 `_) +* Removed ament_cmake dependency (`#989 `_) +* Switch to using new rcutils_strerror (`#993 `_) +* Ensure all rclcpp::Clock accesses are thread-safe +* Use a PIMPL for rclcpp::Clock implementation +* Replace rmw_implementation for rmw dependency in package.xml (`#990 `_) +* Add missing service callback registration tracepoint (`#986 `_) +* Rename rmw_topic_endpoint_info_array count to size (`#996 `_) +* Implement functions to get publisher and subcription informations like QoS policies from topic name (`#960 `_) +* Code style only: wrap after open parenthesis if not in one line (`#977 `_) +* Accept taking an rvalue ref future in spin_until_future_complete (`#971 `_) +* Allow node clock use in logging macros (`#969 `_) (`#970 `_) +* Change order of deprecated and visibility attributes (`#968 `_) +* Deprecated is_initialized() (`#967 `_) +* Don't specify calling convention in std::_Binder template (`#952 `_) +* Added missing include to logging.hpp (`#964 `_) +* Assigning make_shared result to variables in test (`#963 `_) +* Fix unused parameter warning (`#962 `_) +* Stop retaining ownership of the rcl context in GraphListener (`#946 `_) +* Clear sub contexts when starting another init-shutdown cycle (`#947 `_) +* Avoid possible UB in Clock jump callbacks (`#954 `_) +* Handle unknown global ROS arguments (`#951 `_) +* Mark get_clock() as override to fix clang warnings (`#939 `_) +* Create node clock calls const (try 2) (`#922 `_) +* Fix asserts on shared_ptr::use_count; expects long, got uint32 (`#936 `_) +* Use absolute topic name for parameter events (`#929 `_) +* Add enable_rosout into NodeOptions. (`#900 `_) +* Removing "virtual", adding "override" keywords (`#897 `_) +* Use weak_ptr to store context in GraphListener (`#906 `_) +* Complete published event message when declaring a parameter (`#928 `_) +* Fix duration.cpp lint error (`#930 `_) +* Intra-process subscriber should use RMW actual qos. (ros2`#913 `_) (`#914 `_) +* Type conversions fixes (`#901 `_) +* Add override keyword to functions +* Remove unnecessary virtual keywords +* Only check for new work once in spin_some (`#471 `_) (`#844 `_) +* Add addition/subtraction assignment operators to Time (`#748 `_) +* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Barry Xu, Chris Lalancette, Christophe Bedard, Claire Wang, Dan Rose, DensoADAS, Devin Bonnie, Dino Hüllmann, Dirk Thomas, DongheeYe, Emerson Knapp, Ivan Santiago Paunovic, Jacob Perron, Jaison Titus, Karsten Knese, Matt Schickler, Miaofei Mei, Michel Hidalgo, Mikael Arguedas, Monika Idzik, Prajakta Gokhale, Roger Strain, Scott K Logan, Sean Kelly, Stephen Brawner, Steven Macenski, Steven! Ragnarök, Todd Malsbary, Tomoya Fujita, William Woodall, Zachary Michaels + 0.8.3 (2019-11-19) ------------------ diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 4e7f15b511..2513730395 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 0.8.3 + 0.9.0 The ROS client library in C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index f0b1e42e0a..6e6db98551 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,18 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.9.0 (2020-04-29) +------------------ +* Increasing test coverage of rclcpp_action (`#1043 `_) +* Export targets in addition to include directories / libraries (`#1096 `_) +* Deprecate redundant namespaces (`#1083 `_) +* Rename rosidl_generator_c namespace to rosidl_runtime_c (`#1062 `_) +* Changed rosidl_generator_c/cpp to rosidl_runtime_c/cpp (`#1014 `_) +* Fix unknown macro errors reported by cppcheck 1.90 (`#1000 `_) +* Removed rosidl_generator_c dependency (`#992 `_) +* Fix typo in action client logger name (`#937 `_) +* Contributors: Alejandro Hernández Cordero, Dirk Thomas, Jacob Perron, Stephen Brawner, William Woodall + 0.8.3 (2019-11-19) ------------------ * issue-919 Fixed "memory leak" in action clients (`#920 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index d7cc193506..f67b2bc099 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 0.8.3 + 0.9.0 Adds action APIs for C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index e04c00b64a..47601380e0 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.9.0 (2020-04-29) +------------------ +* Added rclcpp_components Doxyfile (`#1091 `_) +* Deprecate redundant namespaces (`#1083 `_) +* Export targets in addition to include directories / libraries (`#1088 `_) +* Export component manager (`#1070 `_) +* Install the component_manager library (`#1068 `_) +* Make Component Manager public (`#1065 `_) +* Remove absolute path from installed CMake code (`#948 `_) +* Fix function docblock, check for unparsed arguments (`#945 `_) +* Contributors: Alejandro Hernández Cordero, DensoADAS, Dirk Thomas, Jacob Perron, Karsten Knese, Michael Carroll, William Woodall + 0.8.3 (2019-11-19) ------------------ diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index ff6cd48590..b6261b58c9 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 0.8.3 + 0.9.0 Package containing tools for dynamically loadable components Michael Carroll Apache License 2.0 diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 8eb18eff0e..085a3ea92e 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,19 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.9.0 (2020-04-29) +------------------ +* Export targets in addition to include directories / libraries (`#1096 `_) +* Deprecate redundant namespaces (`#1083 `_) +* Integrate topic statistics (`#1072 `_) +* Reflect changes in rclcpp API (`#1079 `_) +* Fix unknown macro errors reported by cppcheck 1.90 (`#1000 `_) +* Rremoved rmw_implementation from package.xml (`#991 `_) +* Implement functions to get publisher and subcription informations like QoS policies from topic name (`#960 `_) +* Create node clock calls const (`#922 `_) +* Type conversions fixes (`#901 `_) +* Contributors: Alejandro Hernández Cordero, Barry Xu, Devin Bonnie, Dirk Thomas, Jacob Perron, Monika Idzik, Prajakta Gokhale, Steven Macenski, William Woodall + 0.8.3 (2019-11-19) ------------------ diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 4f5e387e44..a18438b126 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 0.8.3 + 0.9.0 Package containing a prototype for lifecycle implementation Karsten Knese Apache License 2.0 From 084afd6d0097dd680e4c812db61f6255cfbd79c3 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Thu, 30 Apr 2020 09:27:52 -0400 Subject: [PATCH 035/121] Update comment about return value in Executor::get_next_ready_executable (#1085) Signed-off-by: Christophe Bedard --- rclcpp/src/rclcpp/executor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 2ef663baac..878628d901 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -602,7 +602,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) any_executable.callback_group->can_be_taken_from().store(false); } } - // If there is no ready executable, return a null ptr + // If there is no ready executable, return false return success; } From 481b2d2c53b00b03cfdd66ba7b1d021a6516e276 Mon Sep 17 00:00:00 2001 From: brawner Date: Thu, 30 Apr 2020 10:29:45 -0700 Subject: [PATCH 036/121] Increasing test coverage of rclcpp_lifecycle (#1045) Signed-off-by: Stephen Brawner --- rclcpp_lifecycle/CMakeLists.txt | 8 + rclcpp_lifecycle/test/test_lifecycle_node.cpp | 310 ++++++++++++++++ .../test/test_lifecycle_service_client.cpp | 346 ++++++++++++++++++ rclcpp_lifecycle/test/test_state_wrapper.cpp | 28 ++ .../test/test_transition_wrapper.cpp | 44 ++- 5 files changed, 735 insertions(+), 1 deletion(-) create mode 100644 rclcpp_lifecycle/test/test_lifecycle_service_client.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index ab4cf4f857..66df83d10e 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -59,6 +59,14 @@ if(BUILD_TESTING) ) target_link_libraries(test_lifecycle_node ${PROJECT_NAME}) endif() + ament_add_gtest(test_lifecycle_service_client test/test_lifecycle_service_client.cpp) + if(TARGET test_lifecycle_service_client) + ament_target_dependencies(test_lifecycle_service_client + "rcl_lifecycle" + "rclcpp" + ) + target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME}) + endif() ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp) if(TARGET test_state_machine_info) ament_target_dependencies(test_state_machine_info diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index e8066370e0..0e89ba299e 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -14,8 +14,11 @@ #include +#include #include +#include #include +#include #include #include "lifecycle_msgs/msg/state.hpp" @@ -162,6 +165,38 @@ TEST_F(TestDefaultStateMachine, trigger_transition) { TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) { auto test_node = std::make_shared("testnode"); + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + auto ret = reset_key; + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN), ret); + ASSERT_EQ(success, ret); +} + +TEST_F(TestDefaultStateMachine, call_transitions_with_error_code) { + auto test_node = std::make_shared("testnode"); auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -188,6 +223,25 @@ TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) { EXPECT_EQ(success, ret); } +TEST_F(TestDefaultStateMachine, call_transitions_without_code) { + auto test_node = std::make_shared("testnode"); + + auto configured = test_node->configure(); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + + auto activated = test_node->activate(); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + + auto deactivated = test_node->deactivate(); + EXPECT_EQ(deactivated.id(), State::PRIMARY_STATE_INACTIVE); + + auto unconfigured = test_node->cleanup(); + EXPECT_EQ(unconfigured.id(), State::PRIMARY_STATE_UNCONFIGURED); + + auto finalized = test_node->shutdown(); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); +} + TEST_F(TestDefaultStateMachine, good_mood) { auto test_node = std::make_shared>("testnode"); @@ -233,3 +287,259 @@ TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { SUCCEED(); } + +// Parameters are tested more thoroughly in rclcpp's test_node.cpp +// These are provided for coverage of lifecycle node's API +TEST_F(TestDefaultStateMachine, declare_parameters) { + auto test_node = std::make_shared("testnode"); + + auto list_result = test_node->list_parameters({}, 0u); + EXPECT_EQ(list_result.names.size(), 1u); + EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time"); + + const std::string bool_name = "test_boolean"; + const std::string int_name = "test_int"; + + // Default descriptor overload + test_node->declare_parameter(bool_name, rclcpp::ParameterValue(false)); + + // Explicit descriptor overload + rcl_interfaces::msg::ParameterDescriptor int_descriptor; + int_descriptor.name = int_name; + int_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + int_descriptor.description = "Example integer parameter"; + test_node->declare_parameter(int_name, rclcpp::ParameterValue(42), int_descriptor); + + std::map str_parameters; + str_parameters["str_one"] = "stringy_string"; + str_parameters["str_two"] = "stringy_string_string"; + + // Default descriptor overload + test_node->declare_parameters("test_string", str_parameters); + + std::map> double_parameters; + rcl_interfaces::msg::ParameterDescriptor double_descriptor_one; + double_descriptor_one.name = "double_one"; + double_descriptor_one.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + double_parameters["double_one"] = std::make_pair(1.0, double_descriptor_one); + + rcl_interfaces::msg::ParameterDescriptor double_descriptor_two; + double_descriptor_two.name = "double_two"; + double_descriptor_two.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + double_parameters["double_two"] = std::make_pair(2.0, double_descriptor_two); + + // Explicit descriptor overload + test_node->declare_parameters("test_double", double_parameters); + + list_result = test_node->list_parameters({}, 0u); + EXPECT_EQ(list_result.names.size(), 7u); + + // The order of these names is not controlled by lifecycle_node, doing set equality + std::set expected_names = { + "test_boolean", + "test_double.double_one", + "test_double.double_two", + "test_int", + "test_string.str_one", + "test_string.str_two", + "use_sim_time", + }; + std::set actual_names(list_result.names.begin(), list_result.names.end()); + + EXPECT_EQ(expected_names, actual_names); +} + +TEST_F(TestDefaultStateMachine, check_parameters) { + auto test_node = std::make_shared("testnode"); + + auto list_result = test_node->list_parameters({}, 0u); + EXPECT_EQ(list_result.names.size(), 1u); + EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time"); + + const std::string bool_name = "test_boolean"; + const std::string int_name = "test_int"; + std::vector parameter_names = {bool_name, int_name}; + + EXPECT_FALSE(test_node->has_parameter(bool_name)); + EXPECT_FALSE(test_node->has_parameter(int_name)); + EXPECT_THROW( + test_node->get_parameters(parameter_names), + rclcpp::exceptions::ParameterNotDeclaredException); + + // Default descriptor overload + test_node->declare_parameter(bool_name, rclcpp::ParameterValue(true)); + + // Explicit descriptor overload + rcl_interfaces::msg::ParameterDescriptor int_descriptor; + int_descriptor.name = int_name; + int_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + int_descriptor.description = "Example integer parameter"; + test_node->declare_parameter(int_name, rclcpp::ParameterValue(42), int_descriptor); + + // describe parameters + auto descriptors = test_node->describe_parameters(parameter_names); + EXPECT_EQ(descriptors.size(), parameter_names.size()); + + EXPECT_THROW( + test_node->describe_parameter("not_a_real_parameter"), + rclcpp::exceptions::ParameterNotDeclaredException); + + // describe parameter matches explicit descriptor + auto descriptor = test_node->describe_parameter(int_name); + EXPECT_STREQ(descriptor.name.c_str(), int_descriptor.name.c_str()); + EXPECT_EQ(descriptor.type, int_descriptor.type); + EXPECT_STREQ(descriptor.description.c_str(), int_descriptor.description.c_str()); + + // bool parameter exists and value matches + EXPECT_TRUE(test_node->has_parameter(bool_name)); + EXPECT_EQ(test_node->get_parameter(bool_name).as_bool(), true); + + // int parameter exists and value matches + EXPECT_TRUE(test_node->has_parameter(int_name)); + EXPECT_EQ(test_node->get_parameter(int_name).as_int(), 42); + + // Get multiple parameters at a time + auto parameters = test_node->get_parameters(parameter_names); + EXPECT_EQ(parameters.size(), parameter_names.size()); + EXPECT_EQ(parameters[0].as_bool(), true); + EXPECT_EQ(parameters[1].as_int(), 42); + + // Get multiple parameters at a time with map + std::map parameter_map; + EXPECT_TRUE(test_node->get_parameters({}, parameter_map)); + + // int param, bool param, and use_sim_time + EXPECT_EQ(parameter_map.size(), 3u); + + // Check parameter types + auto parameter_types = test_node->get_parameter_types(parameter_names); + EXPECT_EQ(parameter_types.size(), parameter_names.size()); + EXPECT_EQ(parameter_types[0], rcl_interfaces::msg::ParameterType::PARAMETER_BOOL); + EXPECT_EQ(parameter_types[1], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + + // Setting parameters + size_t parameters_set = 0; + auto callback = [¶meters_set](const std::vector & parameters) { + parameters_set += parameters.size(); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; + }; + + test_node->set_on_parameters_set_callback(callback); + rclcpp::Parameter bool_parameter(bool_name, rclcpp::ParameterValue(false)); + EXPECT_TRUE(test_node->set_parameter(bool_parameter).successful); + EXPECT_EQ(parameters_set, 1u); + + rclcpp::Parameter int_parameter(int_name, rclcpp::ParameterValue(7)); + test_node->set_parameters({int_parameter}); + EXPECT_EQ(parameters_set, 2u); + + // List parameters + list_result = test_node->list_parameters({}, 0u); + EXPECT_EQ(list_result.names.size(), 3u); + EXPECT_STREQ(list_result.names[0].c_str(), parameter_names[0].c_str()); + EXPECT_STREQ(list_result.names[1].c_str(), parameter_names[1].c_str()); + EXPECT_STREQ(list_result.names[2].c_str(), "use_sim_time"); + + // Undeclare parameter + test_node->undeclare_parameter(bool_name); + EXPECT_FALSE(test_node->has_parameter(bool_name)); + rclcpp::Parameter parameter; + EXPECT_FALSE(test_node->get_parameter(bool_name, parameter)); + + // Bool parameter has been undeclared, atomic setting should fail + parameters = { + rclcpp::Parameter(bool_name, rclcpp::ParameterValue(true)), + rclcpp::Parameter(int_name, rclcpp::ParameterValue(0))}; + EXPECT_THROW( + test_node->set_parameters_atomically(parameters), + rclcpp::exceptions::ParameterNotDeclaredException); + + // Since setting parameters failed, this should remain the same + EXPECT_EQ(test_node->get_parameter(int_name).as_int(), 7); + + // Bool parameter no longer exists, using "or" value + EXPECT_FALSE( + test_node->get_parameter_or( + bool_name, parameter, rclcpp::Parameter(bool_name, rclcpp::ParameterValue(true)))); + EXPECT_TRUE(parameter.as_bool()); +} + +TEST_F(TestDefaultStateMachine, test_getters) { + auto test_node = std::make_shared("testnode"); + auto options = test_node->get_node_options(); + EXPECT_EQ(0u, options.arguments().size()); + EXPECT_NE(nullptr, test_node->get_node_base_interface()); + EXPECT_NE(nullptr, test_node->get_node_clock_interface()); + EXPECT_NE(nullptr, test_node->get_node_graph_interface()); + EXPECT_NE(nullptr, test_node->get_node_logging_interface()); + EXPECT_NE(nullptr, test_node->get_node_time_source_interface()); + EXPECT_NE(nullptr, test_node->get_node_timers_interface()); + EXPECT_NE(nullptr, test_node->get_node_topics_interface()); + EXPECT_NE(nullptr, test_node->get_node_services_interface()); + EXPECT_NE(nullptr, test_node->get_node_parameters_interface()); + EXPECT_NE(nullptr, test_node->get_node_waitables_interface()); + EXPECT_NE(nullptr, test_node->get_graph_event()); + EXPECT_NE(nullptr, test_node->get_clock()); + EXPECT_LT(0u, test_node->now().nanoseconds()); + EXPECT_STREQ("testnode", test_node->get_logger().get_name()); + EXPECT_NE(nullptr, const_cast(test_node.get())->get_clock()); +} + +TEST_F(TestDefaultStateMachine, test_graph) { + auto test_node = std::make_shared("testnode"); + auto names = test_node->get_node_names(); + EXPECT_EQ(names.size(), 1u); + EXPECT_STREQ(names[0].c_str(), "/testnode"); + + // parameter_events, rosout, /testnode/transition_event + auto topic_names_and_types = test_node->get_topic_names_and_types(); + EXPECT_EQ(topic_names_and_types.size(), 3u); + EXPECT_STREQ( + topic_names_and_types["/testnode/transition_event"][0].c_str(), + "lifecycle_msgs/msg/TransitionEvent"); + + auto service_names_and_types = test_node->get_service_names_and_types(); + EXPECT_EQ(service_names_and_types.size(), 11u); + // These are specific to lifecycle nodes, other services are provided by rclcpp::Node + EXPECT_STREQ( + service_names_and_types["/testnode/change_state"][0].c_str(), + "lifecycle_msgs/srv/ChangeState"); + EXPECT_STREQ( + service_names_and_types["/testnode/get_available_states"][0].c_str(), + "lifecycle_msgs/srv/GetAvailableStates"); + EXPECT_STREQ( + service_names_and_types["/testnode/get_available_transitions"][0].c_str(), + "lifecycle_msgs/srv/GetAvailableTransitions"); + EXPECT_STREQ( + service_names_and_types["/testnode/get_state"][0].c_str(), + "lifecycle_msgs/srv/GetState"); + EXPECT_STREQ( + service_names_and_types["/testnode/get_transition_graph"][0].c_str(), + "lifecycle_msgs/srv/GetAvailableTransitions"); + + EXPECT_EQ(1u, test_node->count_publishers("/testnode/transition_event")); + EXPECT_EQ(0u, test_node->count_subscribers("/testnode/transition_event")); + + auto publishers_info = test_node->get_publishers_info_by_topic("/testnode/transition_event"); + EXPECT_EQ(publishers_info.size(), 1u); + auto subscriptions_info = + test_node->get_subscriptions_info_by_topic("/testnode/transition_event"); + EXPECT_EQ(subscriptions_info.size(), 0u); +} + +TEST_F(TestDefaultStateMachine, test_callback_groups) { + auto test_node = std::make_shared("testnode"); + auto groups = test_node->get_callback_groups(); + EXPECT_EQ(groups.size(), 1u); + + auto group = test_node->create_callback_group( + rclcpp::callback_group::CallbackGroupType::MutuallyExclusive); + EXPECT_NE(nullptr, group); + + groups = test_node->get_callback_groups(); + EXPECT_EQ(groups.size(), 2u); + EXPECT_EQ(groups[1].lock().get(), group.get()); +} diff --git a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp new file mode 100644 index 0000000000..76978d052f --- /dev/null +++ b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp @@ -0,0 +1,346 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** + * Service client test was adopted from: + * https://github.com/ros2/demos/blob/master/lifecycle/src/lifecycle_service_client.cpp + */ + +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" +#include "lifecycle_msgs/msg/transition_description.hpp" +#include "lifecycle_msgs/srv/change_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 "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using namespace std::chrono_literals; + +constexpr char const * lifecycle_node_name = "lc_talker"; + +constexpr char const * node_get_state_topic = "/lc_talker/get_state"; +constexpr char const * node_change_state_topic = "/lc_talker/change_state"; +constexpr char const * node_get_available_states_topic = "/lc_talker/get_available_states"; +constexpr char const * node_get_available_transitions_topic = + "/lc_talker/get_available_transitions"; +constexpr char const * node_get_transition_graph_topic = + "/lc_talker/get_transition_graph"; +const lifecycle_msgs::msg::State unknown_state = lifecycle_msgs::msg::State(); + +class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + EmptyLifecycleNode() + : rclcpp_lifecycle::LifecycleNode(lifecycle_node_name) + {} +}; + +class LifecycleServiceClient : public rclcpp::Node +{ +public: + explicit LifecycleServiceClient(std::string node_name) + : Node(node_name) + { + client_get_available_states_ = this->create_client( + node_get_available_states_topic); + client_get_available_transitions_ = + this->create_client( + node_get_available_transitions_topic); + client_get_transition_graph_ = + this->create_client( + node_get_transition_graph_topic); + client_get_state_ = this->create_client( + node_get_state_topic); + client_change_state_ = this->create_client( + node_change_state_topic); + } + + lifecycle_msgs::msg::State + get_state(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_state_->wait_for_service(time_out)) { + return unknown_state; + } + + auto future_result = client_get_state_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return unknown_state; + } + + if (future_result.get()) { + return future_result.get()->current_state; + } else { + return unknown_state; + } + } + + bool + change_state(std::uint8_t transition, std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + request->transition.id = transition; + + if (!client_change_state_->wait_for_service(time_out)) { + return false; + } + + auto future_result = client_change_state_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return false; + } + + return future_result.get()->success; + } + + std::vector + get_available_states(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_available_states_->wait_for_service(time_out)) { + return std::vector(); + } + + auto future_result = client_get_available_states_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return std::vector(); + } + + if (future_result.get()) { + return future_result.get()->available_states; + } + + return std::vector(); + } + + std::vector + get_available_transitions(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_available_transitions_->wait_for_service(time_out)) { + return std::vector(); + } + + auto future_result = client_get_available_transitions_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return std::vector(); + } + + if (future_result.get()) { + return future_result.get()->available_transitions; + } + + return std::vector(); + } + + std::vector + get_transition_graph(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_transition_graph_->wait_for_service(time_out)) { + return std::vector(); + } + + auto future_result = client_get_transition_graph_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return std::vector(); + } + + if (future_result.get()) { + return future_result.get()->available_transitions; + } + + return std::vector(); + } + +private: + std::shared_ptr> + client_get_available_states_; + std::shared_ptr> + client_get_available_transitions_; + std::shared_ptr> + client_get_transition_graph_; + std::shared_ptr> client_get_state_; + std::shared_ptr> client_change_state_; +}; + + +class TestLifecycleServiceClient : public ::testing::Test +{ +protected: + EmptyLifecycleNode * lifecycle_node() {return lifecycle_node_.get();} + LifecycleServiceClient * lifecycle_client() {return lifecycle_client_.get();} + +private: + void SetUp() override + { + rclcpp::init(0, nullptr); + lifecycle_node_ = std::make_shared(); + lifecycle_client_ = std::make_shared("client"); + spinner_ = std::thread(&TestLifecycleServiceClient::spin, this); + } + + void TearDown() override + { + rclcpp::shutdown(); + spinner_.join(); + } + + void spin() + { + while (rclcpp::ok()) { + rclcpp::spin_some(lifecycle_node_->get_node_base_interface()); + rclcpp::spin_some(lifecycle_client_); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + std::shared_ptr lifecycle_node_; + std::shared_ptr lifecycle_client_; + std::thread spinner_; +}; + + +TEST_F(TestLifecycleServiceClient, construct_destruct) { + EXPECT_NE(nullptr, lifecycle_client()); + EXPECT_NE(nullptr, lifecycle_node()); +} + +TEST_F(TestLifecycleServiceClient, available_states) { + auto states = lifecycle_client()->get_available_states(); + EXPECT_EQ(states.size(), 11u); + EXPECT_EQ(states[0].id, lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); + EXPECT_EQ(states[1].id, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ(states[2].id, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(states[3].id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ(states[4].id, lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ(states[5].id, lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING); + EXPECT_EQ(states[6].id, lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP); + EXPECT_EQ(states[7].id, lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN); + EXPECT_EQ(states[8].id, lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING); + EXPECT_EQ(states[9].id, lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING); + EXPECT_EQ(states[10].id, lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING); +} + +TEST_F(TestLifecycleServiceClient, transition_graph) { + auto transitions = lifecycle_client()->get_transition_graph(); + EXPECT_EQ(transitions.size(), 25u); +} + +TEST_F(TestLifecycleServiceClient, available_transitions) { + auto transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 2u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + EXPECT_EQ( + transitions[1].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); +} + +TEST_F(TestLifecycleServiceClient, lifecycle_transitions) { + EXPECT_EQ( + lifecycle_client()->get_state().id, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 2u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + EXPECT_EQ( + transitions[1].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 3u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); + EXPECT_EQ(transitions[1].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + EXPECT_EQ( + transitions[2].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_INACTIVE_SHUTDOWN); + + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)); + EXPECT_EQ(lifecycle_client()->get_state().id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 2u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + EXPECT_EQ( + transitions[1].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVE_SHUTDOWN); + + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition:: + TRANSITION_DEACTIVATE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 3u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); + EXPECT_EQ(transitions[1].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + EXPECT_EQ( + transitions[2].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_INACTIVE_SHUTDOWN); + + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP)); + EXPECT_EQ( + lifecycle_client()->get_state().id, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 2u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + EXPECT_EQ( + transitions[1].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition:: + TRANSITION_UNCONFIGURED_SHUTDOWN)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 0u); +} diff --git a/rclcpp_lifecycle/test/test_state_wrapper.cpp b/rclcpp_lifecycle/test/test_state_wrapper.cpp index 5d20759367..599ec9562e 100644 --- a/rclcpp_lifecycle/test/test_state_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_state_wrapper.cpp @@ -28,6 +28,17 @@ class TestStateWrapper : public ::testing::Test } }; +class StateDerived : public rclcpp_lifecycle::State +{ +public: + StateDerived(uint8_t id, const std::string & label) + : State(id, label) {} + void expose_reset() + { + reset(); + } +}; + TEST_F(TestStateWrapper, wrapper) { { rclcpp_lifecycle::State state(1, "my_state"); @@ -95,6 +106,10 @@ TEST_F(TestStateWrapper, copy_constructor) { TEST_F(TestStateWrapper, assignment_operator) { auto a = std::make_shared(1, "one"); + *a = *a; + EXPECT_EQ(1, a->id()); + EXPECT_STREQ("one", a->label().c_str()); + auto b = std::make_shared(2, "two"); *b = *a; @@ -166,3 +181,16 @@ TEST_F(TestStateWrapper, assignment_operator4) { delete lc_state1; } + +TEST_F(TestStateWrapper, exceptions) { + EXPECT_THROW((void)rclcpp_lifecycle::State(0, ""), std::runtime_error); + + const rcl_lifecycle_state_t * null_handle = nullptr; + EXPECT_THROW((void)rclcpp_lifecycle::State(null_handle), std::runtime_error); + + auto reset_state = std::make_shared(1, "one"); + reset_state->expose_reset(); + + EXPECT_THROW(reset_state->id(), std::runtime_error); + EXPECT_THROW(reset_state->label(), std::runtime_error); +} diff --git a/rclcpp_lifecycle/test/test_transition_wrapper.cpp b/rclcpp_lifecycle/test/test_transition_wrapper.cpp index b920c74135..9895dc9148 100644 --- a/rclcpp_lifecycle/test/test_transition_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_transition_wrapper.cpp @@ -28,6 +28,19 @@ class TestTransitionWrapper : public ::testing::Test } }; +class TransitionDerived : public rclcpp_lifecycle::Transition +{ +public: + TransitionDerived( + uint8_t id, const std::string & label, + rclcpp_lifecycle::State && start, rclcpp_lifecycle::State && goal) + : Transition(id, label, std::move(start), std::move(goal)) {} + void expose_reset() + { + reset(); + } +}; + TEST_F(TestTransitionWrapper, empty_transition) { auto a = std::make_shared(1, "my_transition"); EXPECT_NO_THROW(a.reset()); @@ -75,7 +88,15 @@ TEST_F(TestTransitionWrapper, copy_constructor) { } TEST_F(TestTransitionWrapper, assignment_operator) { - auto a = std::make_shared(1, "one"); + rclcpp_lifecycle::State start_state(1, "start_state"); + rclcpp_lifecycle::State goal_state(2, "goal_state"); + auto a = std::make_shared( + 1, "one", std::move(start_state), + std::move(goal_state)); + *a = *a; + EXPECT_EQ(1, a->id()); + EXPECT_STREQ("one", a->label().c_str()); + auto b = std::make_shared(2, "two"); *b = *a; @@ -83,4 +104,25 @@ TEST_F(TestTransitionWrapper, assignment_operator) { EXPECT_EQ(1, b->id()); EXPECT_STREQ("one", b->label().c_str()); + EXPECT_STREQ("start_state", b->start_state().label().c_str()); + EXPECT_STREQ("goal_state", b->goal_state().label().c_str()); + EXPECT_EQ(1, b->start_state().id()); + EXPECT_EQ(2, b->goal_state().id()); +} + +TEST_F(TestTransitionWrapper, exceptions) { + rcl_lifecycle_transition_t * null_handle = nullptr; + EXPECT_THROW((void)rclcpp_lifecycle::Transition(null_handle), std::runtime_error); + + rclcpp_lifecycle::State start_state(1, "start_state"); + rclcpp_lifecycle::State goal_state(2, "goal_state"); + auto a = std::make_shared( + 1, "one", std::move(start_state), + std::move(goal_state)); + + a->expose_reset(); + EXPECT_THROW(a->start_state(), std::runtime_error); + EXPECT_THROW(a->goal_state(), std::runtime_error); + EXPECT_THROW(a->id(), std::runtime_error); + EXPECT_THROW(a->label(), std::runtime_error); } From f59a8a62f1535203fb323e1f9f8fbe25698b668c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 1 May 2020 13:55:46 -0300 Subject: [PATCH 037/121] Use RCL_RET_SERVICE_TAKE_FAILED and not RCL_RET_CLIENT_TAKE_FAILED when checking a request take (#1101) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/service.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index fb35a87725..827062bdd3 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -41,7 +41,7 @@ ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & req this->get_service_handle().get(), &request_id_out, request_out); - if (RCL_RET_CLIENT_TAKE_FAILED == ret) { + if (RCL_RET_SERVICE_TAKE_FAILED == ret) { return false; } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); From 7abaa23cd3f09015bda87c6044b8bb02c8221015 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 1 May 2020 22:41:22 +0200 Subject: [PATCH 038/121] Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (#1100) * Added Quality declaration: rclcpp, rclpp_action, rclcpp_components, rclcpp_lifecycle Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde --- README.md | 2 +- rclcpp/QUALITY_DECLARATION.md | 173 +++++++++++++++++++++++ rclcpp/README.md | 7 + rclcpp_action/QUALITY_DECLARATION.md | 166 ++++++++++++++++++++++ rclcpp_action/README.md | 7 + rclcpp_components/QUALITY_DECLARATION.md | 169 ++++++++++++++++++++++ rclcpp_components/README.md | 7 + rclcpp_lifecycle/QUALITY_DECLARATION.md | 167 ++++++++++++++++++++++ rclcpp_lifecycle/README.md | 7 + 9 files changed, 704 insertions(+), 1 deletion(-) create mode 100644 rclcpp/QUALITY_DECLARATION.md create mode 100644 rclcpp/README.md create mode 100644 rclcpp_action/QUALITY_DECLARATION.md create mode 100644 rclcpp_action/README.md create mode 100644 rclcpp_components/QUALITY_DECLARATION.md create mode 100644 rclcpp_components/README.md create mode 100644 rclcpp_lifecycle/QUALITY_DECLARATION.md create mode 100644 rclcpp_lifecycle/README.md diff --git a/README.md b/README.md index 37d875e69c..f219245ab5 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ rclcpp provides the standard C++ API for interacting with ROS 2. `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system. -Visit the [rclcpp API documentation](http://docs.ros2.org/eloquent/api/rclcpp/) for a complete list of its main components. +Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components. ### Examples diff --git a/rclcpp/QUALITY_DECLARATION.md b/rclcpp/QUALITY_DECLARATION.md new file mode 100644 index 0000000000..4ef02b2451 --- /dev/null +++ b/rclcpp/QUALITY_DECLARATION.md @@ -0,0 +1,173 @@ +This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://github.com/ros-infrastructure/rep/blob/rep-2004/rep-2004.rst). + +# `rclcpp` Quality Declaration + +The package `rclcpp` claims to be in the **Quality Level 4** category. + +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. + +## Version Policy [1] + +### Version Scheme [1.i] + +`rclcpp` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning) + +### Version Stability [1.ii] + +`rclcpp` is not yet at a stable version, i.e. `>= 1.0.0`. +The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). + +### Public API Declaration [1.iii] + +All symbols in the installed headers are considered part of the public API. + +Except for the exclusions listed below, all installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private. +Headers under the folder `experimental` are not considered part of the public API as they have not yet been stabilized. These symbols are namespaced `rclcpp::experimental`. +Headers under the folder `detail` are not considered part of the public API and are subject to change without notice. These symbols are namespaced `rclcpp::detail`. + +### API Stability Policy [1.iv] + +`rclcpp` will not break public API within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +### ABI Stability Policy [1.v] + +`rclcpp` contains C++ code and therefore must be concerned with ABI stability, and will maintain ABI stability within a ROS distribution. + +### ABI and ABI Stability Within a Released ROS Distribution [1.vi] + +`rclcpp` will not break API nor ABI within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +## Change Control Process [2] + +`rclcpp` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process). + +### Change Requests [2.i] + +All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. + +### Contributor Origin [2.ii] + +This package uses DCO as its confirmation of contributor origin policy. More information can be found in [CONTRIBUTING](../CONTRIBUTING.md). + +### Peer Review Policy [2.iii] + +All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. + +### Continuous Integration [2.iv] + +All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers) + +Currently nightly results can be seen here: + +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/) + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +`rclcpp` has a [feature list](http://docs.ros2.org/latest/api/rclcpp/) and each item in the list links to the corresponding feature documentation. There is documentation for all of the features, and new features require documentation before being added. + +### Public API Documentation [3.ii] + +The API is publicly available in the [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp/). + +### License [3.iii] + +The license for `rclcpp` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file. + +There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Epr/job/Epr__rclcpp__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package. + +### Copyright Statements [3.iv] + +The copyright holders each provide a statement of copyright in each source code file in `rclcpp`. + +There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Epr/job/Epr__rclcpp__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp/copyright/). + +## Testing [4] + +### Feature Testing [4.i] + +Each feature in `rclcpp` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp/tree/master/test) directory. +New features are required to have tests before being added. + +Currently nightly test results can be seen here: + +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/) + +### Public API Testing [4.ii] + +Each part of the public API has tests, and new additions or changes to the public API require tests before being added. +The tests aim to cover both typical usage and corner cases, but are quantified by contributing to code coverage. + +### Coverage [4.iii] + +`rclcpp` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage. + +This includes: + +- tracking and reporting line coverage statistics +- achieving and maintaining a reasonable branch line coverage (90-100%) +- no lines are manually skipped in coverage calculations + +Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. + +### Performance [4.iv] + +It is not yet defined if this package requires performance testing and how addresses this topic. + +### Linters and Static Analysis [4.v] + +`rclcpp` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. + +Currently nightly test results can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/) + +## Dependencies [5] + +### Direct and Optional Runtime ROS Dependencies [5.i]/[5.ii] + +`rclcpp` has the following runtime ROS dependencies: + - libstatistics_collector + - rcl + - rcl_yaml_param_parser + - rcpputils + - rcutils + - rmw + - statistics_msgs + - tracetools + +It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. +It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. + +### Direct Runtime non-ROS Dependency [5.iii] + +`rclcpp` has no run-time or build-time dependencies that need to be considered for this declaration. + +## Platform Support [6] + +`rclcpp` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them. + +Currently nightly build status can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/rclcpp/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/rclcpp/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/rclcpp/) + +## Security + +### Vulnerability Disclosure Policy [7.i] + +This package does not yet have a Vulnerability Disclosure Policy diff --git a/rclcpp/README.md b/rclcpp/README.md new file mode 100644 index 0000000000..5b6a1dfc09 --- /dev/null +++ b/rclcpp/README.md @@ -0,0 +1,7 @@ +# `rclcpp` + +The ROS client library in C++. Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components. + +## Quality Declaration + +This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rclcpp_action/QUALITY_DECLARATION.md b/rclcpp_action/QUALITY_DECLARATION.md new file mode 100644 index 0000000000..1d81de18b0 --- /dev/null +++ b/rclcpp_action/QUALITY_DECLARATION.md @@ -0,0 +1,166 @@ +This document is a declaration of software quality for the `rclcpp_action` package, based on the guidelines in [REP-2004](https://github.com/ros-infrastructure/rep/blob/rep-2004/rep-2004.rst). + +# `rclcpp_action` Quality Declaration + +The package `rclcpp_action` claims to be in the **Quality Level 4** category. + +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. + +## Version Policy [1] + +### Version Scheme [1.i] + +`rclcpp_action` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning) + +### Version Stability [1.ii] + +`rclcpp_action` is not yet at a stable version, i.e. `>= 1.0.0`. +The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). + +### Public API Declaration [1.iii] + +All symbols in the installed headers are considered part of the public API. + +All installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private. + +### API Stability Policy [1.iv] + +`rclcpp_action` will not break public API within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +### ABI Stability Policy [1.v] + +`rclcpp_action` contains C++ code and therefore must be concerned with ABI stability, and will maintain ABI stability within a ROS distribution. + +### ABI and ABI Stability Within a Released ROS Distribution [1.vi] + +`rclcpp_action` will not break API nor ABI within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +## Change Control Process [2] + +`rclcpp_action` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process). + +### Change Requests [2.i] + +All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. + +### Contributor Origin [2.ii] + +This package uses DCO as its confirmation of contributor origin policy. More information can be found in [CONTRIBUTING](../CONTRIBUTING.md). + +### Peer Review Policy [2.iii] + +All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. + +### Continuous Integration [2.iv] + +All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers) + +Currently nightly results can be seen here: + +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_action/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_action/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_action/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_action/) + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +`rclcpp_action` has a [feature list](http://docs.ros2.org/latest/api/rclcpp_action/) and each item in the list links to the corresponding feature documentation. There is documentation for all of the features, and new features require documentation before being added. + +### Public API Documentation [3.ii] + +The API is publicly available in the [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp_action/). + +### License [3.iii] + +The license for `rclcpp_action` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file. + +There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_action__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_action/) can be found a list with the latest results of the various linters being run on the package. + +### Copyright Statements [3.iv] + +The copyright holders each provide a statement of copyright in each source code file in `rclcpp_action`. + +There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_action__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_action/copyright/). + +## Testing [4] + +### Feature Testing [4.i] + +Each feature in `rclcpp_action` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_action/tree/master/test) directory. +New features are required to have tests before being added. + +Currently nightly test results can be seen here: + +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_action/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_action/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_action/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_action/) + +### Public API Testing [4.ii] + +Each part of the public API has tests, and new additions or changes to the public API require tests before being added. +The tests aim to cover both typical usage and corner cases, but are quantified by contributing to code coverage. + +### Coverage [4.iii] + +`rclcpp_action` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage. + +This includes: + +- tracking and reporting line coverage statistics +- achieving and maintaining a reasonable branch line coverage (90-100%) +- no lines are manually skipped in coverage calculations + +Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. + +### Performance [4.iv] + +It is not yet defined if this package requires performance testing and how addresses this topic. + +### Linters and Static Analysis [4.v] + +`rclcpp_action` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. + +Currently nightly test results can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_action/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_action/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_action/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_action/) + +## Dependencies [5] + +### Direct and Optional Runtime ROS Dependencies [5.i]/[5.ii] + +`rclcpp_action` has the following runtime ROS dependencies: + - action_msgs + - rclcpp + - rcl_action + +It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. +It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. + +### Direct Runtime non-ROS Dependency [5.iii] + +`rclcpp_action` has no run-time or build-time dependencies that need to be considered for this declaration. + +## Platform Support [6] + +`rclcpp_action` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them. + +Currently nightly build status can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp_action/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/rclcpp_action/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/rclcpp_action/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/rclcpp_action/) + +## Security + +### Vulnerability Disclosure Policy [7.i] + +This package does not yet have a Vulnerability Disclosure Policy diff --git a/rclcpp_action/README.md b/rclcpp_action/README.md new file mode 100644 index 0000000000..0d6ccc030a --- /dev/null +++ b/rclcpp_action/README.md @@ -0,0 +1,7 @@ +# `rclcpp_action` + +Adds action APIs for C++. Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp_action/) for a complete list of its main components or the [design document](http://design.ros2.org/articles/actions.html). + +## Quality Declaration + +This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rclcpp_components/QUALITY_DECLARATION.md b/rclcpp_components/QUALITY_DECLARATION.md new file mode 100644 index 0000000000..a63140e671 --- /dev/null +++ b/rclcpp_components/QUALITY_DECLARATION.md @@ -0,0 +1,169 @@ +This document is a declaration of software quality for the `rclcpp_components` package, based on the guidelines in [REP-2004](https://github.com/ros-infrastructure/rep/blob/rep-2004/rep-2004.rst). + +# `rclcpp_components` Quality Declaration + +The package `rclcpp_components` claims to be in the **Quality Level 4** category. + +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. + +## Version Policy [1] + +### Version Scheme [1.i] + +`rclcpp_components` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning) + +### Version Stability [1.ii] + +`rclcpp_components` is not yet at a stable version, i.e. `>= 1.0.0`. +The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). + +### Public API Declaration [1.iii] + +All symbols in the installed headers are considered part of the public API. + +All installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private. + +### API Stability Policy [1.iv] + +`rclcpp_components` will not break public API within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +### ABI Stability Policy [1.v] + +`rclcpp_components` contains C++ code and therefore must be concerned with ABI stability, and will maintain ABI stability within a ROS distribution. + +### ABI and ABI Stability Within a Released ROS Distribution [1.vi] + +`rclcpp_components` will not break API nor ABI within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +## Change Control Process [2] + +`rclcpp_components` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process). + +### Change Requests [2.i] + +All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. + +### Contributor Origin [2.ii] + +This package uses DCO as its confirmation of contributor origin policy. More information can be found in [CONTRIBUTING](../CONTRIBUTING.md). + +### Peer Review Policy [2.iii] + +All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. + +### Continuous Integration [2.iv] + +All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers) + +Currently nightly results can be seen here: + +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_components/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_components/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_components/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_components/) + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +`rclcpp_components` does not have a documented feature list. + +### Public API Documentation [3.ii] + +`rclcpp_components` does not cover a public API documentation. + +The API is publicly available in the [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp_components/). + +### License [3.iii] + +The license for `rclcpp_components` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file. + +There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_components__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_components/) can be found a list with the latest results of the various linters being run on the package. + +### Copyright Statements [3.iv] + +The copyright holders each provide a statement of copyright in each source code file in `rclcpp_components`. + +There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_components__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_components/copyright/). + +## Testing [4] + +### Feature Testing [4.i] + +Each feature in `rclcpp_components` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_components/tree/master/test) directory. +New features are required to have tests before being added. + +Currently nightly test results can be seen here: + +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_components/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_components/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_components/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_components/) + +### Public API Testing [4.ii] + +Each part of the public API has tests, and new additions or changes to the public API require tests before being added. +The tests aim to cover both typical usage and corner cases, but are quantified by contributing to code coverage. + +### Coverage [4.iii] + +`rclcpp_components` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage. + +This includes: + +- tracking and reporting line coverage statistics +- achieving and maintaining a reasonable branch line coverage (90-100%) +- no lines are manually skipped in coverage calculations + +Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. + +### Performance [4.iv] + +It is not yet defined if this package requires performance testing and how addresses this topic. + +### Linters and Static Analysis [4.v] + +`rclcpp_components` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. + +Currently nightly test results can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_components/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_components/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_components/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_components/) + +## Dependencies [5] + +### Direct and Optional Runtime ROS Dependencies [5.i]/[5.ii] + +`rclcpp_components` has the following runtime ROS dependencies: + - ament_index_cpp + - class_loader + - composition_interfaces + - rclcpp + +It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. +It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. + +### Direct Runtime non-ROS Dependency [5.iii] + +`rclcpp_components` has no run-time or build-time dependencies that need to be considered for this declaration. + +## Platform Support [6] + +`rclcpp_components` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them. + +Currently nightly build status can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp_components/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/rclcpp_components/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/rclcpp_components/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/rclcpp_components/) + +## Security + +### Vulnerability Disclosure Policy [7.i] + +This package does not yet have a Vulnerability Disclosure Policy diff --git a/rclcpp_components/README.md b/rclcpp_components/README.md new file mode 100644 index 0000000000..16d6fb97d8 --- /dev/null +++ b/rclcpp_components/README.md @@ -0,0 +1,7 @@ +# `rclcpp_components` + +Package containing tools for dynamically loadable components. + +## Quality Declaration + +This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rclcpp_lifecycle/QUALITY_DECLARATION.md b/rclcpp_lifecycle/QUALITY_DECLARATION.md new file mode 100644 index 0000000000..686176a40c --- /dev/null +++ b/rclcpp_lifecycle/QUALITY_DECLARATION.md @@ -0,0 +1,167 @@ +This document is a declaration of software quality for the `rclcpp_lifecycle` package, based on the guidelines in [REP-2004](https://github.com/ros-infrastructure/rep/blob/rep-2004/rep-2004.rst). + +# `rclcpp_lifecycle` Quality Declaration + +The package `rclcpp_lifecycle` claims to be in the **Quality Level 4** category. + +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. + +## Version Policy [1] + +### Version Scheme [1.i] + +`rclcpp_lifecycle` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning) + +### Version Stability [1.ii] + +`rclcpp_lifecycle` is not yet at a stable version, i.e. `>= 1.0.0`. +The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). + +### Public API Declaration [1.iii] + +All symbols in the installed headers are considered part of the public API. + +All installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private. + +### API Stability Policy [1.iv] + +`rclcpp_lifecycle` will not break public API within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +### ABI Stability Policy [1.v] + +`rclcpp_lifecycle` contains C++ code and therefore must be concerned with ABI stability, and will maintain ABI stability within a ROS distribution. + +### ABI and ABI Stability Within a Released ROS Distribution [1.vi] + +`rclcpp_lifecycle` will not break API nor ABI within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +## Change Control Process [2] + +`rclcpp_lifecycle` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process). + +### Change Requests [2.i] + +All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. + +### Contributor Origin [2.ii] + +This package uses DCO as its confirmation of contributor origin policy. More information can be found in [CONTRIBUTING](../CONTRIBUTING.md). + +### Peer Review Policy [2.iii] + +All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. + +### Continuous Integration [2.iv] + +All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers) + +Currently nightly results can be seen here: + +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_lifecycle/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_lifecycle/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_lifecycle/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_lifecycle/) + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +`rclcpp_lifecycle` does not have a documented feature list. + +### Public API Documentation [3.ii] + +`rclcpp_lifecycle` does not cover a public API documentation. + +### License [3.iii] + +The license for `rclcpp_lifecycle` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file. + +There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_lifecycle__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_lifecycle/) can be found a list with the latest results of the various linters being run on the package. + +### Copyright Statements [3.iv] + +The copyright holders each provide a statement of copyright in each source code file in `rclcpp_lifecycle`. + +There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_lifecycle__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_lifecycle/copyright/). + +## Testing [4] + +### Feature Testing [4.i] + +Each feature in `rclcpp_lifecycle` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_lifecycle/tree/master/test) directory. +New features are required to have tests before being added. + +Currently nightly test results can be seen here: + +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_lifecycle/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_lifecycle/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_lifecycle/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_lifecycle/) + +### Public API Testing [4.ii] + +Each part of the public API has tests, and new additions or changes to the public API require tests before being added. +The tests aim to cover both typical usage and corner cases, but are quantified by contributing to code coverage. + +### Coverage [4.iii] + +`rclcpp_lifecycle` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage. + +This includes: + +- tracking and reporting line coverage statistics +- achieving and maintaining a reasonable branch line coverage (90-100%) +- no lines are manually skipped in coverage calculations + +Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. + +### Performance [4.iv] + +It is not yet defined if this package requires performance testing and how addresses this topic. + +### Linters and Static Analysis [4.v] + +`rclcpp_lifecycle` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. + +Currently nightly test results can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_lifecycle/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_lifecycle/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_lifecycle/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_lifecycle/) + +## Dependencies [5] + +### Direct and Optional Runtime ROS Dependencies [5.i]/[5.ii] + +`rclcpp_lifecycle` has the following runtime ROS dependencies: + - lifecycle_msgs + - rclcpp + - rcl_lifecycle + - rosidl_typesupport_cpp + +It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. +It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. + +### Direct Runtime non-ROS Dependency [5.iii] + +`rclcpp_lifecycle` has no run-time or build-time dependencies that need to be considered for this declaration. + +## Platform Support [6] + +`rclcpp_lifecycle` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them. + +Currently nightly build status can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp_lifecycle/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/rclcpp_lifecycle/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/rclcpp_lifecycle/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/rclcpp_lifecycle/) + +## Security + +### Vulnerability Disclosure Policy [7.i] + +This package does not yet have a Vulnerability Disclosure Policy diff --git a/rclcpp_lifecycle/README.md b/rclcpp_lifecycle/README.md new file mode 100644 index 0000000000..afe67905f7 --- /dev/null +++ b/rclcpp_lifecycle/README.md @@ -0,0 +1,7 @@ +# `rclcpp_lifecycle` + +Package containing a prototype for lifecycle implementation. Visit the [design document](https://design.ros2.org/articles/node_lifecycle.html) for more information about this package. + +## Quality Declaration + +This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. From 133771485c4d0669caf39ffbff84d7ade67659b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 6 May 2020 09:04:31 +0200 Subject: [PATCH 039/121] Added docblock in rclcpp (#1103) * Added docblock in rclcpp Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde --- rclcpp/include/rclcpp/client.hpp | 20 +++++++ rclcpp/include/rclcpp/clock.hpp | 2 + rclcpp/include/rclcpp/duration.hpp | 38 ++++++++++-- rclcpp/include/rclcpp/node.hpp | 19 +++++- rclcpp/include/rclcpp/parameter.hpp | 46 ++++++++++++++ rclcpp/include/rclcpp/publisher.hpp | 11 ++++ rclcpp/include/rclcpp/qos.hpp | 60 +++++++++++++++++++ rclcpp/include/rclcpp/rclcpp.hpp | 1 + rclcpp/include/rclcpp/service.hpp | 12 ++++ rclcpp/include/rclcpp/subscription.hpp | 5 ++ .../include/rclcpp/subscription_options.hpp | 4 ++ rclcpp/include/rclcpp/time.hpp | 50 +++++++++++++++- rclcpp/include/rclcpp/time_source.hpp | 32 +++++++++- rclcpp/include/rclcpp/timer.hpp | 24 ++++++++ 14 files changed, 312 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f7f1d5cb8c..e6b02f9969 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -84,22 +84,42 @@ class ClientBase bool take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out); + /// Return the name of the service. + /** \return The name of the service. */ RCLCPP_PUBLIC const char * get_service_name() const; + /// Return the rcl_client_t client handle in a std::shared_ptr. + /** + * This handle remains valid after the Client is destroyed. + * The actual rcl client is not finalized until it is out of scope everywhere. + */ RCLCPP_PUBLIC std::shared_ptr get_client_handle(); + /// Return the rcl_client_t client handle in a std::shared_ptr. + /** + * This handle remains valid after the Client is destroyed. + * The actual rcl client is not finalized until it is out of scope everywhere. + */ RCLCPP_PUBLIC std::shared_ptr get_client_handle() const; + /// Return if the service is ready. + /** + * \return `true` if the service is ready, `false` otherwise + */ RCLCPP_PUBLIC bool service_is_ready() const; + /// Wait for a service to be ready. + /** + * \param timeout maximum time to wait + */ template bool wait_for_service( diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 3b5b22515a..211966ee41 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -89,6 +89,7 @@ class Clock bool ros_time_is_active(); + /// Return the rcl_clock_t clock handle RCLCPP_PUBLIC rcl_clock_t * get_clock_handle() noexcept; @@ -97,6 +98,7 @@ class Clock rcl_clock_type_t get_clock_type() const noexcept; + /// Get the clock's mutex RCLCPP_PUBLIC std::mutex & get_clock_mutex() noexcept; diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index 5e09cc5cb1..eb716c3348 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -26,11 +26,22 @@ namespace rclcpp class RCLCPP_PUBLIC Duration { public: + /// Duration constructor. + /** + * Initializes the time values for seconds and nanoseconds individually. + * Large values for nsecs are wrapped automatically with the remainder added to secs. + * Both inputs must be integers. + * Seconds can be negative. + * + * \param seconds time in seconds + * \param nanoseconds time in nanoseconds + */ Duration(int32_t seconds, uint32_t nanoseconds); - // This constructor matches any numeric value - ints or floats + // This constructor matches any numeric value - ints or floats. explicit Duration(rcl_duration_value_t nanoseconds); + // This constructor matches std::chrono::nanoseconds. explicit Duration(std::chrono::nanoseconds nanoseconds); // This constructor matches any std::chrono value other than nanoseconds @@ -44,6 +55,10 @@ class RCLCPP_PUBLIC Duration // cppcheck-suppress noExplicitConstructor Duration(const builtin_interfaces::msg::Duration & duration_msg); // NOLINT(runtime/explicit) + /// Time constructor + /** + * \param duration rcl_duration_t structure to copy. + */ explicit Duration(const rcl_duration_t & duration); Duration(const Duration & rhs); @@ -80,6 +95,10 @@ class RCLCPP_PUBLIC Duration Duration operator-(const rclcpp::Duration & rhs) const; + /// Get the maximum representable value. + /** + * \return the maximum representable value + */ static Duration max(); @@ -87,19 +106,27 @@ class RCLCPP_PUBLIC Duration Duration operator*(double scale) const; + /// Get duration in nanosecods + /** + * \return the duration in nanoseconds as a rcl_duration_value_t. + */ 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. + /// Get duration in seconds + /** + * \warning Depending on sizeof(double) there could be significant precision loss. + * When an exact time is required use nanoseconds() instead. + * \return the duration in seconds as a floating point number. + */ double seconds() const; - // Create a duration object from a floating point number representing seconds + /// Create a duration object from a floating point number representing seconds static Duration from_seconds(double seconds); + /// Convert Duration into a std::chrono::Duration. template DurationT to_chrono() const @@ -107,6 +134,7 @@ class RCLCPP_PUBLIC Duration return std::chrono::duration_cast(std::chrono::nanoseconds(this->nanoseconds())); } + /// Convert Duration into rmw_time_t. rmw_time_t to_rmw_time() const; diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 6131fef95a..7279a6a97e 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -186,8 +186,8 @@ class Node : public std::enable_shared_from_this /// Create and return a Subscription. /** * \param[in] topic_name The topic to subscribe on. + * \param[in] qos QoS profile for Subcription. * \param[in] callback The user-defined callback function to receive a message - * \param[in] qos_history_depth The depth of the subscription's incoming message queue. * \param[in] options Additional options for the creation of the Subscription. * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. * \return Shared pointer to the created subscription. @@ -229,7 +229,13 @@ class Node : public std::enable_shared_from_this CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); - /* Create and return a Client. */ + /// Create and return a Client. + /** + * \param[in] service_name The topic to service on. + * \param[in] rmw_qos_profile_t Quality of service profile for client. + * \param[in] group Callback group to call the service. + * \return Shared pointer to the created client. + */ template typename rclcpp::Client::SharedPtr create_client( @@ -237,7 +243,14 @@ class Node : public std::enable_shared_from_this const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, rclcpp::CallbackGroup::SharedPtr group = nullptr); - /* Create and return a Service. */ + /// Create and return a Service. + /** + * \param[in] service_name The topic to service on. + * \param[in] callback User-defined callback function. + * \param[in] rmw_qos_profile_t Quality of service profile for client. + * \param[in] group Callback group to call the service. + * \return Shared pointer to the created service. + */ template typename rclcpp::Service::SharedPtr create_service( diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index e01d5aa772..68147ed731 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -83,18 +83,22 @@ class Parameter bool operator!=(const Parameter & rhs) const; + /// Get the type of the parameter RCLCPP_PUBLIC ParameterType get_type() const; + /// Get the type name of the parameter RCLCPP_PUBLIC std::string get_type_name() const; + /// Get the name of the parameter RCLCPP_PUBLIC const std::string & get_name() const; + /// Get value of parameter as a parameter message. RCLCPP_PUBLIC rcl_interfaces::msg::ParameterValue get_value_message() const; @@ -105,6 +109,9 @@ class Parameter get_parameter_value() const; /// Get value of parameter using rclcpp::ParameterType as template argument. + /** + * \throws rclcpp::exceptions::InvalidParameterTypeException if the type doesn't match + */ template decltype(auto) get_value() const @@ -117,50 +124,89 @@ class Parameter decltype(auto) get_value() const; + /// Get value of parameter as boolean. + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC bool as_bool() const; + /// Get value of parameter as integer. + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC int64_t as_int() const; + /// Get value of parameter as double. + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC double as_double() const; + /// Get value of parameter as string. + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC const std::string & as_string() const; + /// Get value of parameter as byte array (vector). + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC const std::vector & as_byte_array() const; + /// Get value of parameter as bool array (vector). + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC const std::vector & as_bool_array() const; + /// Get value of parameter as integer array (vector). + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC const std::vector & as_integer_array() const; + /// Get value of parameter as double array (vector). + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC const std::vector & as_double_array() const; + /// Get value of parameter as string array (vector). + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC const std::vector & as_string_array() const; + /// Convert a parameter message in a Parameter class object. RCLCPP_PUBLIC static Parameter from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter); + /// Convert the class in a parameter message. RCLCPP_PUBLIC rcl_interfaces::msg::Parameter to_parameter_msg() const; + /// Get value of parameter as a string. RCLCPP_PUBLIC std::string value_to_string() const; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index abe259b31d..ebe349cd99 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -59,6 +59,17 @@ class Publisher : public PublisherBase RCLCPP_SMART_PTR_DEFINITIONS(Publisher) + /// Default constructor. + /** + * The constructor for a Publisher is almost never called directly. + * Instead, subscriptions should be instantiated through the function + * rclcpp::create_publisher(). + * + * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup. + * \param[in] topic Name of the topic to publish to. + * \param[in] qos QoS profile for Subcription. + * \param[in] options options for the subscription. + */ Publisher( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index 244ac699dc..ac016efec5 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -161,6 +161,18 @@ bool operator==(const QoS & left, const QoS & right); RCLCPP_PUBLIC bool operator!=(const QoS & left, const QoS & right); +/** + * Sensor Data QoS class + * - History: Keep last, + * - Depth: 5, + * - Reliability: Best effort, + * - Durability: Volatile, + * - Deadline: Default, + * - Lifespan: Default, + * - Liveliness: System default, + * - Liveliness lease duration: default, + * - avoid ros namespace conventions: false + */ class RCLCPP_PUBLIC SensorDataQoS : public QoS { public: @@ -171,6 +183,18 @@ class RCLCPP_PUBLIC SensorDataQoS : public QoS )); }; +/** + * Parameters QoS class + * - History: Keep last, + * - Depth: 1000, + * - Reliability: Reliable, + * - Durability: Volatile, + * - Deadline: Default, + * - Lifespan: Default, + * - Liveliness: System default, + * - Liveliness lease duration: default, + * - Avoid ros namespace conventions: false + */ class RCLCPP_PUBLIC ParametersQoS : public QoS { public: @@ -181,6 +205,18 @@ class RCLCPP_PUBLIC ParametersQoS : public QoS )); }; +/** + * Services QoS class + * - History: Keep last, + * - Depth: 10, + * - Reliability: Reliable, + * - Durability: Volatile, + * - Deadline: Default, + * - Lifespan: Default, + * - Liveliness: System default, + * - Liveliness lease duration: default, + * - Avoid ros namespace conventions: false + */ class RCLCPP_PUBLIC ServicesQoS : public QoS { public: @@ -191,6 +227,18 @@ class RCLCPP_PUBLIC ServicesQoS : public QoS )); }; +/** + * Parameter events QoS class + * - History: Keep last, + * - Depth: 1000, + * - Reliability: Reliable, + * - Durability: Volatile, + * - Deadline: Default, + * - Lifespan: Default, + * - Liveliness: System default, + * - Liveliness lease duration: default, + * - Avoid ros namespace conventions: false + */ class RCLCPP_PUBLIC ParameterEventsQoS : public QoS { public: @@ -201,6 +249,18 @@ class RCLCPP_PUBLIC ParameterEventsQoS : public QoS )); }; +/** + * System defaults QoS class + * - History: System default, + * - Depth: System default, + * - Reliability: System default, + * - Durability: System default, + * - Deadline: Default, + * - Lifespan: Default, + * - Liveliness: System default, + * - Liveliness lease duration: System default, + * - Avoid ros namespace conventions: false + */ class RCLCPP_PUBLIC SystemDefaultsQoS : public QoS { public: diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 556085ee07..dc22c7e649 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -128,6 +128,7 @@ * - rclcpp/context.hpp * - rclcpp/contexts/default_context.hpp * - Various utilities: + * - rclcpp/duration.hpp * - rclcpp/function_traits.hpp * - rclcpp/macros.hpp * - rclcpp/scope_exit.hpp diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index de720109a6..abe25867f0 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -50,14 +50,26 @@ class ServiceBase RCLCPP_PUBLIC virtual ~ServiceBase(); + /// Return the name of the service. + /** \return The name of the service. */ RCLCPP_PUBLIC const char * get_service_name(); + /// Return the rcl_service_t service handle in a std::shared_ptr. + /** + * This handle remains valid after the Service is destroyed. + * The actual rcl service is not finalized until it is out of scope everywhere. + */ RCLCPP_PUBLIC std::shared_ptr get_service_handle(); + /// Return the rcl_service_t service handle in a std::shared_ptr. + /** + * This handle remains valid after the Service is destroyed. + * The actual rcl service is not finalized until it is out of scope everywhere. + */ RCLCPP_PUBLIC std::shared_ptr get_service_handle() const; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2b8108fc94..97e8714b40 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -90,9 +90,14 @@ class Subscription : public SubscriptionBase * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup. * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. * \param[in] topic_name Name of the topic to subscribe to. + * \param[in] qos QoS profile for Subcription. * \param[in] callback User defined callback to call when a message is received. * \param[in] options options for the subscription. * \param[in] message_memory_strategy The memory strategy to be used for managing message memory. + * \param[in] subscription_topic_statistics pointer to a topic statistics subcription. + * \throws std::invalid_argument if the QoS is uncompatible with intra-process (if one + * of the following conditions are true: qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL, + * qos_profile.depth == 0 or qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE). */ Subscription( rclcpp::node_interfaces::NodeBaseInterface * node_base, diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 12275d74fe..5088f35fb1 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -89,6 +89,10 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase {} /// Convert this class, with a rclcpp::QoS, into an rcl_subscription_options_t. + /** + * \param qos QoS profile for subcription. + * \return rcl_subscription_options_t structure based on the rclcpp::QoS + */ template rcl_subscription_options_t to_rcl_subscription_options(const rclcpp::QoS & qos) const diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index d8f65c8672..a80201a356 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -31,26 +31,54 @@ class Clock; class Time { public: + /// Time constructor + /** + * Initializes the time values for seconds and nanoseconds individually. + * Large values for nanoseconds are wrapped automatically with the remainder added to seconds. + * Both inputs must be integers. + * + * \param seconds part of the time in seconds since time epoch + * \param nanoseconds part of the time in nanoseconds since time epoch + * \param clock_type clock type + */ RCLCPP_PUBLIC Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); + /// Time constructor + /** + * \param nanoseconds since time epoch + * \param clock_type clock type + */ RCLCPP_PUBLIC explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME); + /// Copy constructor RCLCPP_PUBLIC Time(const Time & rhs); + /// Time constructor + /** + * \param time_msg builtin_interfaces time message to copy + * \param clock_type clock type + */ RCLCPP_PUBLIC Time( const builtin_interfaces::msg::Time & time_msg, rcl_clock_type_t ros_time = RCL_ROS_TIME); + /// Time constructor + /** + * \param time_point rcl_time_point_t structure to copy + * \param clock_type clock type + */ RCLCPP_PUBLIC explicit Time(const rcl_time_point_t & time_point); + /// Time destructor RCLCPP_PUBLIC virtual ~Time(); + /// Return a builtin_interfaces::msg::Time object based RCLCPP_PUBLIC operator builtin_interfaces::msg::Time() const; @@ -106,21 +134,37 @@ class Time Time & operator-=(const rclcpp::Duration & rhs); + /// Get the nanoseconds since epoch + /** + * \return the nanoseconds since epoch as a rcl_time_point_value_t structure. + */ RCLCPP_PUBLIC rcl_time_point_value_t nanoseconds() const; + /// Get the maximum representable value. + /** + * \return the maximum representable value + */ RCLCPP_PUBLIC 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. + /// Get the seconds since epoch + /** + * \warning Depending on sizeof(double) there could be significant precision loss. + * When an exact time is required use nanoseconds() instead. + * + * \return the seconds since epoch as a floating point number. + */ RCLCPP_PUBLIC double seconds() const; + /// Get the clock type + /** + * \return the clock type + */ RCLCPP_PUBLIC rcl_clock_type_t get_clock_type() const; diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 9dd4023d42..86f62a6cab 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -35,15 +35,42 @@ class Clock; class TimeSource { public: + /// Constructor + /** + * The node will be attached to the time source. + * + * \param node std::shared pointer to a initialized node + */ RCLCPP_PUBLIC explicit TimeSource(rclcpp::Node::SharedPtr node); + /// Empty constructor + /** + * An Empty TimeSource class + */ RCLCPP_PUBLIC TimeSource(); + /// Attack node to the time source. + /** + * \param node std::shared pointer to a initialized node + */ RCLCPP_PUBLIC void attachNode(rclcpp::Node::SharedPtr node); + /// Attack node to the time source. + /** + * If the parameter `use_sim_time` is `true` then the source time is the simulation time, + * otherwise the source time is defined by the system. + * + * \param node_base_interface Node base interface. + * \param node_topics_interface Node topic base interface. + * \param node_graph_interface Node graph interface. + * \param node_services_interface Node service interface. + * \param node_logging_interface Node logging interface. + * \param node_clock_interface Node clock interface. + * \param node_parameters_interface Node parameters interface. + */ RCLCPP_PUBLIC void attachNode( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, @@ -54,19 +81,22 @@ class TimeSource rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface); + /// Detach the node from the time source RCLCPP_PUBLIC void detachNode(); /// Attach a clock to the time source to be updated /** - * \throws std::invalid_argument if node is nullptr + * \throws std::invalid_argument the time source must be a RCL_ROS_TIME otherwise throws an exception */ RCLCPP_PUBLIC void attachClock(rclcpp::Clock::SharedPtr clock); + /// Detach a clock to the time source RCLCPP_PUBLIC void detachClock(rclcpp::Clock::SharedPtr clock); + /// TimeSource Destructor RCLCPP_PUBLIC ~TimeSource(); diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index dd754c0748..3f3b4d80b7 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -48,15 +48,26 @@ class TimerBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase) + /// TimerBase constructor + /** + * \param clock A clock to use for time and sleeping + * \param period The interval at which the timer fires + * \param context node context + */ RCLCPP_PUBLIC explicit TimerBase( Clock::SharedPtr clock, std::chrono::nanoseconds period, rclcpp::Context::SharedPtr context); + /// TimerBase destructor RCLCPP_PUBLIC ~TimerBase(); + /// Cancel the timer. + /** + * \throws std::runtime_error if the rcl_timer_cancel returns a failure + */ RCLCPP_PUBLIC void cancel(); @@ -71,10 +82,15 @@ class TimerBase bool is_canceled(); + /// Reset the timer. + /** + * \throws std::runtime_error if the rcl_timer_reset returns a failure + */ RCLCPP_PUBLIC void reset(); + /// Call the callback function when the timer signal is emitted. RCLCPP_PUBLIC virtual void execute_callback() = 0; @@ -209,6 +225,8 @@ class GenericTimer : public TimerBase callback_(*this); } + /// Is the clock steady (i.e. is the time between ticks constant?) + /** \return True if the clock used by this timer is steady. */ bool is_steady() override { @@ -233,6 +251,12 @@ class WallTimer : public GenericTimer public: RCLCPP_SMART_PTR_DEFINITIONS(WallTimer) + /// Wall timer constructor + /** + * \param period The interval at which the timer fires + * \param callback The callback function to execute every interval + * \param context node context + */ WallTimer( std::chrono::nanoseconds period, FunctorT && callback, From 8fc0c375aaa12c7618a3ecbf11c9fb641d6c19d5 Mon Sep 17 00:00:00 2001 From: Devin Bonnie <47613035+dabonnie@users.noreply.github.com> Date: Wed, 6 May 2020 09:04:45 -0700 Subject: [PATCH 040/121] Fix tests that were not properly torn down (#1073) Signed-off-by: Devin Bonnie --- rclcpp/test/test_publisher_subscription_count_api.cpp | 6 +++++- rclcpp/test/test_subscription_publisher_count_api.cpp | 4 ++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/test_publisher_subscription_count_api.cpp b/rclcpp/test/test_publisher_subscription_count_api.cpp index c709ff9bd0..cbcd4f12ed 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" @@ -51,6 +51,10 @@ class TestPublisherSubscriptionCount : public ::testing::TestWithParam Date: Fri, 8 May 2020 08:48:52 +0200 Subject: [PATCH 041/121] Added rclcpp lifecycle Doxyfile (#1089) * Added rclcpp lifecycle Doxyfile Signed-off-by: ahcorde * Fixed doxyfile Signed-off-by: ahcorde --- rclcpp_lifecycle/Doxyfile | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 rclcpp_lifecycle/Doxyfile diff --git a/rclcpp_lifecycle/Doxyfile b/rclcpp_lifecycle/Doxyfile new file mode 100644 index 0000000000..9a83ee5e85 --- /dev/null +++ b/rclcpp_lifecycle/Doxyfile @@ -0,0 +1,33 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rclcpp_lifecycle" +PROJECT_NUMBER = master +PROJECT_BRIEF = "C++ ROS Lifecycle Library API" + +# Use these lines to include the generated logging.hpp (update install path if needed) +# Otherwise just generate for the local (non-generated header files) + + +INPUT = ./include + +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED = RCLCPP_LIFECYCLE_PUBLIC= + +# Tag files that do not exist will produce a warning and cross-project linking will not work. +TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) +TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" +TAGFILES += "../../../../doxygen_tag_files/rcl_lifecycle.tag=http://docs.ros2.org/latest/api/rcl_lifecycle/" +TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +# Uncomment to generate tag files for cross-project linking. +GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp_lifecycle.tag" From 8b238e69322ffa17d057f7d088b4a51feb779cd4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 8 May 2020 15:39:57 +0000 Subject: [PATCH 042/121] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 9 +++++++++ rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/CHANGELOG.rst | 7 +++++++ 4 files changed, 26 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index f3ee617c58..0cdf1cb9f5 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix tests that were not properly torn down (`#1073 `_) +* Added docblock in rclcpp (`#1103 `_) +* Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) +* Use RCL_RET_SERVICE_TAKE_FAILED and not RCL_RET_CLIENT_TAKE_FAILED when checking a request take (`#1101 `_) +* Update comment about return value in Executor::get_next_ready_executable (`#1085 `_) +* Contributors: Alejandro Hernández Cordero, Christophe Bedard, Devin Bonnie, Ivan Santiago Paunovic + 0.9.0 (2020-04-29) ------------------ * Serialized message move constructor (`#1097 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 6e6db98551..19fef15186 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) +* Contributors: Alejandro Hernández Cordero + 0.9.0 (2020-04-29) ------------------ * Increasing test coverage of rclcpp_action (`#1043 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 47601380e0..6310196c2f 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) +* Contributors: Alejandro Hernández Cordero + 0.9.0 (2020-04-29) ------------------ * Added rclcpp_components Doxyfile (`#1091 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 085a3ea92e..f30958b99e 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added rclcpp lifecycle Doxyfile (`#1089 `_) +* Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) +* Increasing test coverage of rclcpp_lifecycle (`#1045 `_) +* Contributors: Alejandro Hernández Cordero, brawner + 0.9.0 (2020-04-29) ------------------ * Export targets in addition to include directories / libraries (`#1096 `_) From 674cf40bf622d14ea507337711db303fb9fb61b5 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 8 May 2020 15:40:05 +0000 Subject: [PATCH 043/121] 0.9.1 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 0cdf1cb9f5..27e932d366 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.9.1 (2020-05-08) +------------------ * Fix tests that were not properly torn down (`#1073 `_) * Added docblock in rclcpp (`#1103 `_) * Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 2513730395..f109218fbb 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 0.9.0 + 0.9.1 The ROS client library in C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 19fef15186..ef2acbe5bc 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.9.1 (2020-05-08) +------------------ * Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) * Contributors: Alejandro Hernández Cordero diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index f67b2bc099..54762cada4 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 0.9.0 + 0.9.1 Adds action APIs for C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 6310196c2f..0410cc060c 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.9.1 (2020-05-08) +------------------ * Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) * Contributors: Alejandro Hernández Cordero diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index b6261b58c9..54f2849dc1 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 0.9.0 + 0.9.1 Package containing tools for dynamically loadable components Michael Carroll Apache License 2.0 diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index f30958b99e..a07f6fb68c 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.9.1 (2020-05-08) +------------------ * Added rclcpp lifecycle Doxyfile (`#1089 `_) * Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) * Increasing test coverage of rclcpp_lifecycle (`#1045 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index a18438b126..4265a48ba4 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 0.9.0 + 0.9.1 Package containing a prototype for lifecycle implementation Karsten Knese Apache License 2.0 From 1404c23d378f6855b0725a1100e26ac475eacfe1 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 8 May 2020 09:04:38 -0700 Subject: [PATCH 044/121] avoid callback_group deprecation (#1108) Signed-off-by: Karsten Knese --- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 0e89ba299e..57d8b63bf0 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -536,7 +536,7 @@ TEST_F(TestDefaultStateMachine, test_callback_groups) { EXPECT_EQ(groups.size(), 1u); auto group = test_node->create_callback_group( - rclcpp::callback_group::CallbackGroupType::MutuallyExclusive); + rclcpp::CallbackGroupType::MutuallyExclusive); EXPECT_NE(nullptr, group); groups = test_node->get_callback_groups(); From 0ed3ad1135d4fc86c1abb46d6dcaa462a22433a0 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 11 May 2020 09:16:04 -0700 Subject: [PATCH 045/121] Mark flaky test with xfail: TestMultiThreadedExecutor (#1109) Signed-off-by: Louise Poubel --- rclcpp/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index ef2f106c34..6eb02dfdeb 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -604,6 +604,7 @@ if(BUILD_TESTING) ament_target_dependencies(test_multi_threaded_executor "rcl") target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) + ament_add_test_label(test_multi_threaded_executor xfail) endif() ament_add_gtest(test_guard_condition test/test_guard_condition.cpp From 3f4980c41f0dddf268d30eca8486c389fdb77404 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 11 May 2020 13:32:49 -0400 Subject: [PATCH 046/121] Make sure to include what you use. (#1112) Signed-off-by: Chris Lalancette --- rclcpp/src/rclcpp/publisher_base.cpp | 4 +++- rclcpp/src/rclcpp/subscription_base.cpp | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index c982a95d33..a10f2e17f7 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -35,6 +35,7 @@ #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/qos_event.hpp" using rclcpp::PublisherBase; @@ -246,7 +247,8 @@ PublisherBase::setup_intra_process( } void -PublisherBase::default_incompatible_qos_callback(QOSOfferedIncompatibleQoSInfo & event) const +PublisherBase::default_incompatible_qos_callback( + rclcpp::QOSOfferedIncompatibleQoSInfo & event) const { std::string policy_name = qos_policy_name_from_kind(event.last_policy_kind); RCLCPP_WARN( diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index de1acfadb2..dc8c559ffa 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -24,6 +24,7 @@ #include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/qos_event.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -238,7 +239,8 @@ SubscriptionBase::get_intra_process_waitable() const } void -SubscriptionBase::default_incompatible_qos_callback(QOSRequestedIncompatibleQoSInfo & event) const +SubscriptionBase::default_incompatible_qos_callback( + rclcpp::QOSRequestedIncompatibleQoSInfo & event) const { std::string policy_name = qos_policy_name_from_kind(event.last_policy_kind); RCLCPP_WARN( From a0a727f16450bd4295d52f085aadd2120b1ab7d9 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 12 May 2020 08:29:45 -0700 Subject: [PATCH 047/121] use rosidl_default_generators dependency in test (#1114) Signed-off-by: Karsten Knese --- rclcpp/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 6eb02dfdeb..d7463f7a2a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -12,7 +12,6 @@ find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(rmw REQUIRED) find_package(rosgraph_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) find_package(rosidl_runtime_cpp REQUIRED) find_package(rosidl_typesupport_c REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) @@ -217,6 +216,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(rmw_implementation_cmake REQUIRED) + find_package(rosidl_default_generators REQUIRED) find_package(test_msgs REQUIRED) From 767f54249b4f2f9428de0ad654f2e9cb19492424 Mon Sep 17 00:00:00 2001 From: brawner Date: Tue, 12 May 2020 10:26:49 -0700 Subject: [PATCH 048/121] Increasing test coverage of rclcpp_components (#1044) * Increasing test coverage of rclcpp_components Signed-off-by: Stephen Brawner * PR fixup Signed-off-by: Stephen Brawner * Fixup Signed-off-by: Stephen Brawner * Removing throws test for now Signed-off-by: Stephen Brawner --- rclcpp_components/CMakeLists.txt | 8 ++ .../test/components/test_component.cpp | 1 - .../test/test_component_manager.cpp | 5 ++ .../test/test_component_manager_api.cpp | 75 ++++++++++++++++++- 4 files changed, 84 insertions(+), 5 deletions(-) diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index b4330964f1..759e0f3acb 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -78,11 +78,19 @@ if(BUILD_TESTING) set(components "${components}test_rclcpp_components::TestComponentBar;$\n") set(components "${components}test_rclcpp_components::TestComponentNoNode;$\n") + # A properly formed resource only has one ';', this is used to catch an invalid resource entry + set(invalid_components "test_rclcpp_components::TestComponentFoo;;$\n") + file(GENERATE OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$/share/ament_index/resource_index/rclcpp_components/${PROJECT_NAME}" CONTENT "${components}") + file(GENERATE + OUTPUT + "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$/share/ament_index/resource_index/rclcpp_components/invalid_${PROJECT_NAME}" + CONTENT "${invalid_components}") + set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") if(WIN32) set(append_library_dirs "${append_library_dirs}/$") diff --git a/rclcpp_components/test/components/test_component.cpp b/rclcpp_components/test/components/test_component.cpp index dc85187650..7193ed7e1f 100644 --- a/rclcpp_components/test/components/test_component.cpp +++ b/rclcpp_components/test/components/test_component.cpp @@ -55,7 +55,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.cpp b/rclcpp_components/test/test_component_manager.cpp index a19fdab1ba..a0515094cf 100644 --- a/rclcpp_components/test/test_component_manager.cpp +++ b/rclcpp_components/test/test_component_manager.cpp @@ -88,4 +88,9 @@ TEST_F(TestComponentManager, create_component_factory_invalid) auto resources = manager->get_component_resources("rclcpp_components"); auto factory = manager->create_component_factory({"foo_class", resources[0].second}); EXPECT_EQ(nullptr, factory); + + // Test improperly formed resources file + EXPECT_THROW( + auto resources = manager->get_component_resources("invalid_rclcpp_components"), + rclcpp_components::ComponentManagerException); } diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index a13bf22ec4..138fc38124 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -143,6 +143,65 @@ TEST_F(TestComponentManager, components_api) EXPECT_EQ(result.get()->unique_id, 0u); } + { + // Remap rules + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_remap"; + request->remap_rules.push_back("alice:=bob"); + + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result.get()->success, true); + EXPECT_EQ(result.get()->error_message, ""); + EXPECT_EQ(result.get()->full_node_name, "/test_component_remap"); + EXPECT_EQ(result.get()->unique_id, 5u); + } + + { + // use_intra_process_comms + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_intra_process"; + rclcpp::Parameter use_intraprocess_comms("use_intra_process_comms", + rclcpp::ParameterValue(true)); + request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); + + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result.get()->success, true); + EXPECT_EQ(result.get()->error_message, ""); + std::cout << result.get()->full_node_name << std::endl; + EXPECT_EQ(result.get()->full_node_name, "/test_component_intra_process"); + EXPECT_EQ(result.get()->unique_id, 6u); + } + + { + // use_intra_process_comms is not a bool type parameter + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_intra_process_str"; + + rclcpp::Parameter use_intraprocess_comms("use_intra_process_comms", + rclcpp::ParameterValue("hello")); + request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); + + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result.get()->success, false); + EXPECT_EQ( + result.get()->error_message, + "Extra component argument 'use_intra_process_comms' must be a boolean"); + EXPECT_EQ(result.get()->full_node_name, ""); + EXPECT_EQ(result.get()->unique_id, 0u); + } + auto node_names = node->get_node_names(); auto find_in_nodes = [node_names](std::string name) { @@ -170,16 +229,20 @@ TEST_F(TestComponentManager, components_api) auto node_names = result.get()->full_node_names; auto unique_ids = result.get()->unique_ids; - EXPECT_EQ(node_names.size(), 4u); + EXPECT_EQ(node_names.size(), 6u); EXPECT_EQ(node_names[0], "/test_component_foo"); EXPECT_EQ(node_names[1], "/test_component_bar"); EXPECT_EQ(node_names[2], "/test_component_baz"); EXPECT_EQ(node_names[3], "/ns/test_component_bing"); - EXPECT_EQ(unique_ids.size(), 4u); + EXPECT_EQ(node_names[4], "/test_component_remap"); + EXPECT_EQ(node_names[5], "/test_component_intra_process"); + EXPECT_EQ(unique_ids.size(), 6u); EXPECT_EQ(unique_ids[0], 1u); EXPECT_EQ(unique_ids[1], 2u); EXPECT_EQ(unique_ids[2], 3u); EXPECT_EQ(unique_ids[3], 4u); + EXPECT_EQ(unique_ids[4], 5u); + EXPECT_EQ(unique_ids[5], 6u); } } @@ -230,14 +293,18 @@ TEST_F(TestComponentManager, components_api) auto node_names = result.get()->full_node_names; auto unique_ids = result.get()->unique_ids; - EXPECT_EQ(node_names.size(), 3u); + EXPECT_EQ(node_names.size(), 5u); EXPECT_EQ(node_names[0], "/test_component_bar"); EXPECT_EQ(node_names[1], "/test_component_baz"); EXPECT_EQ(node_names[2], "/ns/test_component_bing"); - EXPECT_EQ(unique_ids.size(), 3u); + EXPECT_EQ(node_names[3], "/test_component_remap"); + EXPECT_EQ(node_names[4], "/test_component_intra_process"); + EXPECT_EQ(unique_ids.size(), 5u); EXPECT_EQ(unique_ids[0], 2u); EXPECT_EQ(unique_ids[1], 3u); EXPECT_EQ(unique_ids[2], 4u); + EXPECT_EQ(unique_ids[3], 5u); + EXPECT_EQ(unique_ids[4], 6u); } } } From a568829a3fecfbbd5bf7d6335c106bdad564b69b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 12 May 2020 14:52:26 -0300 Subject: [PATCH 049/121] Remove MANUAL_BY_NODE liveliness API (#1107) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/node.hpp | 13 ------------- rclcpp/include/rclcpp/node_interfaces/node_base.hpp | 5 ----- .../rclcpp/node_interfaces/node_base_interface.hpp | 6 ------ rclcpp/src/rclcpp/node.cpp | 6 ------ rclcpp/src/rclcpp/node_interfaces/node_base.cpp | 6 ------ rclcpp/test/test_node.cpp | 2 +- rclcpp/test/test_qos.cpp | 2 +- 7 files changed, 2 insertions(+), 38 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7279a6a97e..d54ea21c5e 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -1124,19 +1124,6 @@ class Node : public std::enable_shared_from_this const rclcpp::NodeOptions & get_node_options() const; - /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE). - /** - * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE, the creator - * of this node may manually call `assert_liveliness` at some point in time to signal to the rest - * of the system that this Node is still alive. - * - * \return `true` if the liveliness was asserted successfully, otherwise `false` - */ - RCLCPP_PUBLIC - RCUTILS_WARN_UNUSED - bool - assert_liveliness() const; - protected: /// Construct a sub-node, which will extend the namespace of all entities created with it. /** diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 73861df680..0e124f6f56 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -91,11 +91,6 @@ class NodeBase : public NodeBaseInterface RCLCPP_PUBLIC - bool - assert_liveliness() const override; - - RCLCPP_PUBLIC - rclcpp::CallbackGroup::SharedPtr create_callback_group(rclcpp::CallbackGroupType group_type) override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index 33e4ee3462..bcb469c0bf 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -102,12 +102,6 @@ class NodeBaseInterface 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; - /// Create and return a callback group. RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 29a25eb645..30804809a4 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -499,9 +499,3 @@ Node::get_node_options() const { return this->node_options_; } - -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 0bdf2342e5..f055abaa25 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -202,12 +202,6 @@ NodeBase::get_shared_rcl_node_handle() const return node_handle_; } -bool -NodeBase::assert_liveliness() const -{ - return RCL_RET_OK == rcl_node_assert_liveliness(get_rcl_node_handle()); -} - rclcpp::CallbackGroup::SharedPtr NodeBase::create_callback_group(rclcpp::CallbackGroupType group_type) { diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp index e4851e17d8..3d034651ae 100644 --- a/rclcpp/test/test_node.cpp +++ b/rclcpp/test/test_node.cpp @@ -2591,7 +2591,7 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) { RMW_QOS_POLICY_DURABILITY_VOLATILE, {15, 1678}, {29, 2345}, - RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE, + RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, {5, 23456}, false }; diff --git a/rclcpp/test/test_qos.cpp b/rclcpp/test/test_qos.cpp index b80c7763ca..7188119ceb 100644 --- a/rclcpp/test/test_qos.cpp +++ b/rclcpp/test/test_qos.cpp @@ -65,7 +65,7 @@ TEST(TestQoS, equality_liveliness) { EXPECT_NE(a, b); b.liveliness_lease_duration(duration); EXPECT_EQ(a, b); - a.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE); + a.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); EXPECT_NE(a, b); } From ea6024ec5717f82d3a5727f5d47bc050a49d9201 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 12 May 2020 14:05:31 -0700 Subject: [PATCH 050/121] 1.0.0 --- rclcpp/CHANGELOG.rst | 8 ++++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 13 +++++++++++++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 33 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 27e932d366..96f3b49d67 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2020-05-12) +------------------ +* Remove MANUAL_BY_NODE liveliness API (`#1107 `_) +* Use rosidl_default_generators dependency in test (`#1114 `_) +* Make sure to include what you use (`#1112 `_) +* Mark flaky test with xfail: TestMultiThreadedExecutor (`#1109 `_) +* Contributors: Chris Lalancette, Ivan Santiago Paunovic, Karsten Knese, Louise Poubel + 0.9.1 (2020-05-08) ------------------ * Fix tests that were not properly torn down (`#1073 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index f109218fbb..5303db96e6 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 0.9.1 + 1.0.0 The ROS client library in C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index ef2acbe5bc..20871ce281 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2020-05-12) +------------------ + 0.9.1 (2020-05-08) ------------------ * Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 54762cada4..3f9c9c2c47 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 0.9.1 + 1.0.0 Adds action APIs for C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 0410cc060c..d8de325198 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2020-05-12) +------------------ +* Increasing test coverage of rclcpp_components (`#1044 `_) + * Increasing test coverage of rclcpp_components + Signed-off-by: Stephen Brawner + * PR fixup + Signed-off-by: Stephen Brawner + * Fixup + Signed-off-by: Stephen Brawner + * Removing throws test for now + Signed-off-by: Stephen Brawner +* Contributors: brawner + 0.9.1 (2020-05-08) ------------------ * Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 54f2849dc1..d7e87847a2 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 0.9.1 + 1.0.0 Package containing tools for dynamically loadable components Michael Carroll Apache License 2.0 diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index a07f6fb68c..3e743be5f2 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2020-05-12) +------------------ +* Avoid callback_group deprecation (`#1108 `_) +* Contributors: Karsten Knese + 0.9.1 (2020-05-08) ------------------ * Added rclcpp lifecycle Doxyfile (`#1089 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 4265a48ba4..811de170d4 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 0.9.1 + 1.0.0 Package containing a prototype for lifecycle implementation Karsten Knese Apache License 2.0 From 63e6d43a1736a2e0afe4fa5e14179abfe8e7bc71 Mon Sep 17 00:00:00 2001 From: brawner Date: Tue, 12 May 2020 17:55:49 -0700 Subject: [PATCH 051/121] Update QDs to reflect version 1.0 (#1115) Signed-off-by: Stephen Brawner --- rclcpp/QUALITY_DECLARATION.md | 2 +- rclcpp_action/QUALITY_DECLARATION.md | 2 +- rclcpp_components/QUALITY_DECLARATION.md | 2 +- rclcpp_lifecycle/QUALITY_DECLARATION.md | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/QUALITY_DECLARATION.md b/rclcpp/QUALITY_DECLARATION.md index 4ef02b2451..6fe97bd643 100644 --- a/rclcpp/QUALITY_DECLARATION.md +++ b/rclcpp/QUALITY_DECLARATION.md @@ -14,7 +14,7 @@ Below are the rationales, notes, and caveats for this claim, organized by each r ### Version Stability [1.ii] -`rclcpp` is not yet at a stable version, i.e. `>= 1.0.0`. +`rclcpp` is at a stable version, i.e. `>= 1.0.0`. The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). ### Public API Declaration [1.iii] diff --git a/rclcpp_action/QUALITY_DECLARATION.md b/rclcpp_action/QUALITY_DECLARATION.md index 1d81de18b0..ba27649f8e 100644 --- a/rclcpp_action/QUALITY_DECLARATION.md +++ b/rclcpp_action/QUALITY_DECLARATION.md @@ -14,7 +14,7 @@ Below are the rationales, notes, and caveats for this claim, organized by each r ### Version Stability [1.ii] -`rclcpp_action` is not yet at a stable version, i.e. `>= 1.0.0`. +`rclcpp_action` is at a stable version, i.e. `>= 1.0.0`. The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). ### Public API Declaration [1.iii] diff --git a/rclcpp_components/QUALITY_DECLARATION.md b/rclcpp_components/QUALITY_DECLARATION.md index a63140e671..031316d4d2 100644 --- a/rclcpp_components/QUALITY_DECLARATION.md +++ b/rclcpp_components/QUALITY_DECLARATION.md @@ -14,7 +14,7 @@ Below are the rationales, notes, and caveats for this claim, organized by each r ### Version Stability [1.ii] -`rclcpp_components` is not yet at a stable version, i.e. `>= 1.0.0`. +`rclcpp_components` is at a stable version, i.e. `>= 1.0.0`. The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). ### Public API Declaration [1.iii] diff --git a/rclcpp_lifecycle/QUALITY_DECLARATION.md b/rclcpp_lifecycle/QUALITY_DECLARATION.md index 686176a40c..ac44888df4 100644 --- a/rclcpp_lifecycle/QUALITY_DECLARATION.md +++ b/rclcpp_lifecycle/QUALITY_DECLARATION.md @@ -14,7 +14,7 @@ Below are the rationales, notes, and caveats for this claim, organized by each r ### Version Stability [1.ii] -`rclcpp_lifecycle` is not yet at a stable version, i.e. `>= 1.0.0`. +`rclcpp_lifecycle` is at a stable version, i.e. `>= 1.0.0`. The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). ### Public API Declaration [1.iii] From bdd88a3b8dd285e9bdf132944d609e8de4684622 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 13 May 2020 08:17:56 +0200 Subject: [PATCH 052/121] Added dockblock to ComponentManager class (#1102) * Added dockblock to ComponentManager class Signed-off-by: ahcorde * added feedback Signed-off-by: ahcorde --- .../rclcpp_components/component_manager.hpp | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/rclcpp_components/include/rclcpp_components/component_manager.hpp b/rclcpp_components/include/rclcpp_components/component_manager.hpp index 20d2d7b890..5e9074937b 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager.hpp @@ -40,6 +40,7 @@ class ClassLoader; namespace rclcpp_components { +/// Thrown when an error happens in the component Manager class. class ComponentManagerException : public std::runtime_error { public: @@ -47,6 +48,7 @@ class ComponentManagerException : public std::runtime_error : std::runtime_error(error_desc) {} }; +/// ComponentManager handles the services to load, unload, and get the list of loaded components. class ComponentManager : public rclcpp::Node { public: @@ -60,6 +62,15 @@ class ComponentManager : public rclcpp::Node */ using ComponentResource = std::pair; + /// Default constructor + /** + * Initializes the component manager. It creates the services: load node, unload node + * and list nodes. + * + * \param executor the executor which will spin the node. + * \param node_name the name of the node that the data originates from. + * \param node_options additional options to control creation of the node. + */ RCLCPP_COMPONENTS_PUBLIC ComponentManager( std::weak_ptr executor, @@ -70,17 +81,41 @@ class ComponentManager : public rclcpp::Node virtual ~ComponentManager(); /// Return a list of valid loadable components in a given package. + /* + * \param package_name name of the package + * \param resource_index name of the executable + * \throws ComponentManagerException if the resource was not found or a invalid resource entry + * \return a list of component resources + */ RCLCPP_COMPONENTS_PUBLIC virtual std::vector get_component_resources( const std::string & package_name, const std::string & resource_index = "rclcpp_components") const; + /// Instantiate a component from a dynamic library. + /* + * \param resource a component resource (class name + library path) + * \return a NodeFactory interface + */ RCLCPP_COMPONENTS_PUBLIC virtual std::shared_ptr create_component_factory(const ComponentResource & resource); protected: + /// Service callback to load a new node in the component + /* + * This function allows to add parameters, remap rules, a specific node, name a namespace + * and/or additional arguments. + * + * \param request_header unused + * \param request information with the node to load + * \param response + * \throws std::overflow_error if node_id suffers an overflow. Very unlikely to happen at 1 kHz + * (very optimistic rate). it would take 585 years. + * \throws ComponentManagerException In the case that the component constructor throws an + * exception, rethrow into the following catch block. + */ RCLCPP_COMPONENTS_PUBLIC virtual void OnLoadNode( @@ -88,6 +123,13 @@ class ComponentManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + /// Service callback to unload a node in the component + /* + * \param request_header unused + * \param request unique identifier to remove from the component + * \param response true on the success field if the node unload was succefully, otherwise false + * and the error_message field contains the error. + */ RCLCPP_COMPONENTS_PUBLIC virtual void OnUnloadNode( @@ -95,6 +137,14 @@ class ComponentManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + /// Service callback to get the list of nodes in the component + /* + * Return a two list: one with the unique identifiers and other with full name of the nodes. + * + * \param request_header unused + * \param request unused + * \param response list with the unique ids and full node names + */ RCLCPP_COMPONENTS_PUBLIC virtual void OnListNodes( From 73355da682bdc5e15a0c66187a592367d06bfad4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 13 May 2020 15:47:05 +0200 Subject: [PATCH 053/121] [Quality Declaration] Fixed rep links and added more details to dependencies (#1116) * Fixed rep links and added more details to dependencies Signed-off-by: ahcorde * Fixed rep link Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde --- rclcpp/QUALITY_DECLARATION.md | 65 ++++++++++++++++++++---- rclcpp_action/QUALITY_DECLARATION.md | 30 ++++++++--- rclcpp_components/QUALITY_DECLARATION.md | 37 +++++++++++--- rclcpp_lifecycle/QUALITY_DECLARATION.md | 37 +++++++++++--- 4 files changed, 138 insertions(+), 31 deletions(-) diff --git a/rclcpp/QUALITY_DECLARATION.md b/rclcpp/QUALITY_DECLARATION.md index 6fe97bd643..176771251b 100644 --- a/rclcpp/QUALITY_DECLARATION.md +++ b/rclcpp/QUALITY_DECLARATION.md @@ -1,4 +1,4 @@ -This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://github.com/ros-infrastructure/rep/blob/rep-2004/rep-2004.rst). +This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). # `rclcpp` Quality Declaration @@ -137,20 +137,63 @@ Currently nightly test results can be seen here: ## Dependencies [5] +Below are evaluations of each of `rclcpp`'s run-time and build-time dependencies that have been determined to influence the quality. + +It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. + +It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. + ### Direct and Optional Runtime ROS Dependencies [5.i]/[5.ii] `rclcpp` has the following runtime ROS dependencies: - - libstatistics_collector - - rcl - - rcl_yaml_param_parser - - rcpputils - - rcutils - - rmw - - statistics_msgs - - tracetools -It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. -It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. +#### `libstatistics_collector` + +The `libstatistics_collector` package provides lightweight aggregation utilities to collect statistics and measure message metrics. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md). + +#### `rcl` + +`rcl` a library to support implementation of language specific ROS 2 Client Libraries. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md). + +#### `rcl_yaml_param_parser` + +The `rcl_yaml_param_parser` package provides an API that is used to parse YAML configuration files which may be used to configure ROS and specific nodes. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md). + +#### `rcpputils` + +The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md). + +#### `rcutils` + +The `rcutils` package provides an API which contains common utilities and data structures useful when programming in C. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md). + +#### `rmw` + +`rmw` is the ROS 2 middleware library. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md). + +#### `statistics_msgs` + +The `statistics_msgs` package contains ROS 2 message definitions for reporting statistics for topics and system resources. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md). + +#### `tracetools` + +The `tracetools` package provides utilities for instrumenting the code in `rcl` so that it may be traced for debugging and performance analysis. + +It is **Quality Level 4**, see its [Quality Declaration document](https://gitlab.com/micro-ROS/ros_tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_action/QUALITY_DECLARATION.md b/rclcpp_action/QUALITY_DECLARATION.md index ba27649f8e..fc15b0abf7 100644 --- a/rclcpp_action/QUALITY_DECLARATION.md +++ b/rclcpp_action/QUALITY_DECLARATION.md @@ -1,4 +1,4 @@ -This document is a declaration of software quality for the `rclcpp_action` package, based on the guidelines in [REP-2004](https://github.com/ros-infrastructure/rep/blob/rep-2004/rep-2004.rst). +This document is a declaration of software quality for the `rclcpp_action` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). # `rclcpp_action` Quality Declaration @@ -135,15 +135,33 @@ Currently nightly test results can be seen here: ## Dependencies [5] +Below are evaluations of each of `rclcpp_action`'s run-time and build-time dependencies that have been determined to influence the quality. + +It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. + +It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. + ### Direct and Optional Runtime ROS Dependencies [5.i]/[5.ii] `rclcpp_action` has the following runtime ROS dependencies: - - action_msgs - - rclcpp - - rcl_action -It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. -It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. +#### `action_msgs` + +`action_msgs` provides messages and services for ROS 2 actions. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/QUALITY_DECLARATION.md). + +#### `rclcpp` + +The `rclcpp` package provides the ROS client library in C++. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). + +#### `rcl_action` + +The `rcl_action` package provides C-based ROS action implementation. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_action/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_components/QUALITY_DECLARATION.md b/rclcpp_components/QUALITY_DECLARATION.md index 031316d4d2..74e50d113e 100644 --- a/rclcpp_components/QUALITY_DECLARATION.md +++ b/rclcpp_components/QUALITY_DECLARATION.md @@ -1,4 +1,4 @@ -This document is a declaration of software quality for the `rclcpp_components` package, based on the guidelines in [REP-2004](https://github.com/ros-infrastructure/rep/blob/rep-2004/rep-2004.rst). +This document is a declaration of software quality for the `rclcpp_components` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). # `rclcpp_components` Quality Declaration @@ -137,16 +137,39 @@ Currently nightly test results can be seen here: ## Dependencies [5] +Below are evaluations of each of `rclcpp_components`'s run-time and build-time dependencies that have been determined to influence the quality. + +It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. + +It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. + ### Direct and Optional Runtime ROS Dependencies [5.i]/[5.ii] `rclcpp_components` has the following runtime ROS dependencies: - - ament_index_cpp - - class_loader - - composition_interfaces - - rclcpp -It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. -It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. +#### `ament_index_cpp` + +The `ament_index_cpp` package provides a C++ API to access the ament resource index. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ament/ament_index/blob/master/ament_index_cpp/QUALITY_DECLARATION.md). + +#### `class_loader` + +The `class_loader` package provides a ROS-independent package for loading plugins during runtime + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros/class_loader/blob/ros2/QUALITY_DECLARATION.md). + +#### `composition_interfaces` + +The `composition_interfaces` package contains message and service definitions for managing composable nodes in a container process. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/composition_interfaces/QUALITY_DECLARATION.md). + +#### `rclcpp` + +The `rclcpp` package provides the ROS client library in C++. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_lifecycle/QUALITY_DECLARATION.md b/rclcpp_lifecycle/QUALITY_DECLARATION.md index ac44888df4..afcc1430b7 100644 --- a/rclcpp_lifecycle/QUALITY_DECLARATION.md +++ b/rclcpp_lifecycle/QUALITY_DECLARATION.md @@ -1,4 +1,4 @@ -This document is a declaration of software quality for the `rclcpp_lifecycle` package, based on the guidelines in [REP-2004](https://github.com/ros-infrastructure/rep/blob/rep-2004/rep-2004.rst). +This document is a declaration of software quality for the `rclcpp_lifecycle` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). # `rclcpp_lifecycle` Quality Declaration @@ -135,16 +135,39 @@ Currently nightly test results can be seen here: ## Dependencies [5] +Below are evaluations of each of `rclcpp_lifecycle`'s run-time and build-time dependencies that have been determined to influence the quality. + +It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. + +It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. + ### Direct and Optional Runtime ROS Dependencies [5.i]/[5.ii] `rclcpp_lifecycle` has the following runtime ROS dependencies: - - lifecycle_msgs - - rclcpp - - rcl_lifecycle - - rosidl_typesupport_cpp -It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. -It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. +#### `lifecycle_msgs` + +The `lifecycle_msgs` package contains message and service definitions for managing lifecycle nodes. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/QUALITY_DECLARATION.md). + +#### `rclcpp` + +The `rclcpp` package provides the ROS client library in C++. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). + +#### `rcl_lifecycle` + +The `rcl_lifecycle` package provides functionality for ROS 2 lifecycle nodes in C. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_lifecycle/QUALITY_DECLARATION.md). + +#### `rosidl_typesupport_cpp` + +The `rosidl_typesupport_cpp` package generates the type support for C++ messages. + +It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rosidl_typesupport/blob/master/rosidl_typesupport_cpp/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] From 4608485317ec55a484bc4f31c298c956ef2f567d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 14 May 2020 14:32:23 -0300 Subject: [PATCH 054/121] Make test multi threaded executor more reliable (#1105) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/CMakeLists.txt | 1 - rclcpp/test/executors/test_multi_threaded_executor.cpp | 9 +++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index d7463f7a2a..7e5702cd79 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -604,7 +604,6 @@ if(BUILD_TESTING) ament_target_dependencies(test_multi_threaded_executor "rcl") target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) - ament_add_test_label(test_multi_threaded_executor xfail) endif() ament_add_gtest(test_guard_condition test/test_guard_condition.cpp diff --git a/rclcpp/test/executors/test_multi_threaded_executor.cpp b/rclcpp/test/executors/test_multi_threaded_executor.cpp index e4d648190f..bfe2b25457 100644 --- a/rclcpp/test/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/executors/test_multi_threaded_executor.cpp @@ -34,6 +34,10 @@ class TestMultiThreadedExecutor : public ::testing::Test } }; +constexpr std::chrono::milliseconds PERIOD_MS = 1000ms; +constexpr double PERIOD = PERIOD_MS.count() / 1000.0; +constexpr double TOLERANCE = PERIOD / 4.0; + /* Test that timers are not taken multiple times when using reentrant callback groups. */ @@ -70,9 +74,6 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { // 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++; @@ -92,7 +93,7 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { } }; - auto timer = node->create_wall_timer(100ms, timer_callback, cbg); + auto timer = node->create_wall_timer(PERIOD_MS, timer_callback, cbg); executor.add_node(node); executor.spin(); } From efe2eab82616e598da5934d412871076a9813bcd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 15 May 2020 16:41:25 +0200 Subject: [PATCH 055/121] Added features to rclcpp packages (#1106) * Added features to rclcpp packages Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde * Added feedback and improved lifecycle docblock Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde * Added ffedback Signed-off-by: ahcorde * Fixing error Signed-off-by: ahcorde --- rclcpp/README.md | 4 +- rclcpp_action/README.md | 4 +- rclcpp_components/README.md | 2 + .../rclcpp_components/component_manager.hpp | 26 ++ rclcpp_lifecycle/README.md | 4 +- .../rclcpp_lifecycle/lifecycle_node.hpp | 229 +++++++++++++++++- .../include/rclcpp_lifecycle/state.hpp | 23 ++ .../include/rclcpp_lifecycle/transition.hpp | 40 +++ 8 files changed, 321 insertions(+), 11 deletions(-) diff --git a/rclcpp/README.md b/rclcpp/README.md index 5b6a1dfc09..dff8e5889c 100644 --- a/rclcpp/README.md +++ b/rclcpp/README.md @@ -1,6 +1,8 @@ # `rclcpp` -The ROS client library in C++. Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components. +The ROS client library in C++. + +Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components and features. ## Quality Declaration diff --git a/rclcpp_action/README.md b/rclcpp_action/README.md index 0d6ccc030a..45e14e7f3d 100644 --- a/rclcpp_action/README.md +++ b/rclcpp_action/README.md @@ -1,6 +1,8 @@ # `rclcpp_action` -Adds action APIs for C++. Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp_action/) for a complete list of its main components or the [design document](http://design.ros2.org/articles/actions.html). +Adds action APIs for C++. + +Visit the [rclcpp_action API documentation](http://docs.ros2.org/latest/api/rclcpp_action/) for a complete list of its main components and features. For more information about Actions in ROS 2, see the [design document](http://design.ros2.org/articles/actions.html). ## Quality Declaration diff --git a/rclcpp_components/README.md b/rclcpp_components/README.md index 16d6fb97d8..9abeef1d1e 100644 --- a/rclcpp_components/README.md +++ b/rclcpp_components/README.md @@ -2,6 +2,8 @@ Package containing tools for dynamically loadable components. +Visit the [rclcpp_components API documentation](http://docs.ros2.org/latest/api/rclcpp_components/) for a complete list of its main components and features. + ## Quality Declaration This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rclcpp_components/include/rclcpp_components/component_manager.hpp b/rclcpp_components/include/rclcpp_components/component_manager.hpp index 5e9074937b..202086bdc7 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager.hpp @@ -12,6 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. +/** \mainpage rclcpp_components: Package containing tools for dynamically loadable components. + * + * - ComponentManager: Node to manage components. It has the services to load, unload and list + * current components. + * - rclcpp_components/component_manager.hpp) + * - Node factory: The NodeFactory interface is used by the class loader to instantiate components. + * - rclcpp_components/node_factory.hpp) + * - It allows for classes not derived from `rclcpp::Node` to be used as components. + * - It allows derived constructors to be called when components are loaded. + * + * Some useful abstractions and utilities: + * - [RCLCPP_COMPONENTS_REGISTER_NODE: Register a component that can be dynamically loaded + * at runtime. + * - (include/rclcpp_components/register_node_macro.hpp) + * + * Some useful internal abstractions and utilities: + * - Macros for controlling symbol visibility on the library + * - rclcpp_components/visibility_control.h + * + * Package containing CMake tools for register components: + * - `rclcpp_components_register_node` Register an rclcpp component with the ament resource index + * and create an executable. + * - `rclcpp_components_register_nodes` Register an rclcpp component with the ament resource index. + * The passed library can contain multiple nodes each registered via macro. + */ + #ifndef RCLCPP_COMPONENTS__COMPONENT_MANAGER_HPP__ #define RCLCPP_COMPONENTS__COMPONENT_MANAGER_HPP__ diff --git a/rclcpp_lifecycle/README.md b/rclcpp_lifecycle/README.md index afe67905f7..b4e51e5dff 100644 --- a/rclcpp_lifecycle/README.md +++ b/rclcpp_lifecycle/README.md @@ -1,6 +1,8 @@ # `rclcpp_lifecycle` -Package containing a prototype for lifecycle implementation. Visit the [design document](https://design.ros2.org/articles/node_lifecycle.html) for more information about this package. +Package containing a prototype for lifecycle implementation. + +Visit the [rclcpp_lifecycle API documentation](http://docs.ros2.org/latest/api/rclcpp_lifecycle/) for a complete list of its main components and features. For more information about LifeCycle in ROS 2, see the [design document](http://design.ros2.org/articles/node_lifecycle.html). ## Quality Declaration diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 73d728ba41..7ebf833fb1 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -12,6 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. +/** \mainpage rclcpp_lifecycle: Package containing a prototype for lifecycle implementation. + * + * - Lifecycle states: Define the State class. There are 4 primary states: Unconfigured, Inactive, + * Active and Finalized. There are also 6 transition states which are intermediate states during + * a requested transition. Configuring, CleaningUp, ShuttingDown, Activating, Deactivating and + * ErrorProcessing. + * - rclcpp_lifecycle/state.hpp + * - Lifecycle transitions Define the Transition class. There are 7 transitions exposed to a + * supervisory process, they are: create, configure, cleanup, activate, deactivate, shutdown and + * destroy. + * - rclcpp_lifecycle/transition.hpp + * - Lifecycle publisher creates a publisher that allows enabling and disabling message publication. + * - rclcpp_lifecycle/publisher.hpp + * - Lifecycle node: An optional interface class for life cycle node implementations. + * - rclcpp_lifecycle/lifecycle_node.hpp + * + * Some useful internal abstractions and utilities: + * - Macros for controlling symbol visibility on the library + * - rclcpp_lifecycle/visibility_control.h + */ + #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ @@ -127,29 +148,42 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, virtual ~LifecycleNode(); /// Get the name of the node. - // \return The name of the node. + /** + * \return The name of the node. + */ RCLCPP_LIFECYCLE_PUBLIC const char * get_name() const; - /// Get the namespace of the node. - // \return The namespace of the node. + /// Get the namespace of the node + /** + * \return The namespace of the node. + */ RCLCPP_LIFECYCLE_PUBLIC const char * get_namespace() const; /// Get the logger of the node. - /** \return The logger of the node. */ + /** + * \return The logger of the node. + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::Logger get_logger() const; /// Create and return a callback group. + /** + * \param[in] group_type callback group type to create by this method. + * \return a callback group + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::CallbackGroup::SharedPtr create_callback_group(rclcpp::CallbackGroupType group_type); /// Return the list of callback groups in the node. + /** + * \return list of callback groups in the node. + */ RCLCPP_LIFECYCLE_PUBLIC const std::vector & get_callback_groups() const; @@ -216,7 +250,10 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); - /* Create and return a Client. */ + /// Create and return a Client. + /** + * \sa rclcpp::Node::create_client + */ template typename rclcpp::Client::SharedPtr create_client( @@ -224,7 +261,10 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, rclcpp::CallbackGroup::SharedPtr group = nullptr); - /* Create and return a Service. */ + /// Create and return a Service. + /** + * \sa rclcpp::Node::create_service + */ template typename rclcpp::Service::SharedPtr create_service( @@ -419,22 +459,42 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, set_on_parameters_set_callback( rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType callback); + /// Return a vector of existing node names (string). + /** + * \sa rclcpp::Node::get_node_names + */ RCLCPP_LIFECYCLE_PUBLIC std::vector get_node_names() const; + /// Return a map of existing topic names to list of topic types. + /** + * \sa rclcpp::Node::get_topic_names_and_types + */ RCLCPP_LIFECYCLE_PUBLIC std::map> get_topic_names_and_types(bool no_demangle = false) const; + /// Return a map of existing service names to list of topic types. + /** + * \sa rclcpp::Node::get_service_names_and_types + */ RCLCPP_LIFECYCLE_PUBLIC std::map> get_service_names_and_types() const; + /// Return the number of publishers that are advertised on a given topic. + /** + * \sa rclcpp::Node::count_publishers + */ RCLCPP_LIFECYCLE_PUBLIC size_t count_publishers(const std::string & topic_name) const; + /// Return the number of subscribers who have created a subscription for a given topic. + /** + * \sa rclcpp::Node::count_subscribers + */ RCLCPP_LIFECYCLE_PUBLIC size_t count_subscribers(const std::string & topic_name) const; @@ -477,69 +537,114 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout); + /// Get a clock as a non-const shared pointer which is managed by the node. + /** + * \sa rclcpp::node_interfaces::NodeClock::get_clock + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::Clock::SharedPtr get_clock(); + /// Get a clock as a const shared pointer which is managed by the node. + /** + * \sa rclcpp::node_interfaces::NodeClock::get_clock + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::Clock::ConstSharedPtr get_clock() const; + /// Returns current time from the time source specified by clock_type. + /** + * \sa rclcpp::Clock::now + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::Time now() const; /// Return the Node's internal NodeBaseInterface implementation. + /** + * \sa rclcpp::Node::get_node_base_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); /// Return the Node's internal NodeClockInterface implementation. + /** + * \sa rclcpp::Node::get_node_clock_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeClockInterface::SharedPtr get_node_clock_interface(); /// Return the Node's internal NodeGraphInterface implementation. + /** + * \sa rclcpp::Node::get_node_graph_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeGraphInterface::SharedPtr get_node_graph_interface(); /// Return the Node's internal NodeLoggingInterface implementation. + /** + * \sa rclcpp::Node::get_node_logging_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr get_node_logging_interface(); /// Return the Node's internal NodeTimersInterface implementation. + /** + * \sa rclcpp::Node::get_node_timers_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeTimersInterface::SharedPtr get_node_timers_interface(); /// Return the Node's internal NodeTopicsInterface implementation. + /** + * \sa rclcpp::Node::get_node_topics_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr get_node_topics_interface(); /// Return the Node's internal NodeServicesInterface implementation. + /** + * \sa rclcpp::Node::get_node_services_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeServicesInterface::SharedPtr get_node_services_interface(); /// Return the Node's internal NodeParametersInterface implementation. + /** + * \sa rclcpp::Node::get_node_parameters_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface(); /// Return the Node's internal NodeParametersInterface implementation. + /** + * \sa rclcpp::Node::get_node_time_source_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface(); /// Return the Node's internal NodeWaitablesInterface implementation. + /** + * \sa rclcpp::Node::get_node_waitables_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr get_node_waitables_interface(); /// Return the NodeOptions used when creating this node. + /** + * \sa rclcpp::Node::get_node_options + */ RCLCPP_LIFECYCLE_PUBLIC const rclcpp::NodeOptions & get_node_options() const; @@ -547,100 +652,208 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, // // LIFECYCLE COMPONENTS // + /// Return the current State. + /** + * \return the current state + */ RCLCPP_LIFECYCLE_PUBLIC const State & get_current_state(); + /// Return a list with the available states. + /** + * \return list with the available states. + */ RCLCPP_LIFECYCLE_PUBLIC std::vector get_available_states(); + /// Return a list with the available transitions. + /** + * \return list with the available transitions. + */ RCLCPP_LIFECYCLE_PUBLIC std::vector get_available_transitions(); - /// trigger the specified transition + /// Trigger the specified transition. /* - * return the new state after this transition + * \return the new state after this transition */ RCLCPP_LIFECYCLE_PUBLIC const State & trigger_transition(const Transition & transition); + /// Trigger the specified transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & trigger_transition( const Transition & transition, LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the specified transition based on an id. + /* + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & trigger_transition(uint8_t transition_id); + /// Trigger the specified transition based on an id and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & trigger_transition( uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the configure transition + /* + * \param[out] cb_return_code transition callback return code. + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & configure(); + /// Trigger the configure transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & configure(LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the cleanup transition. + /* + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & cleanup(); + /// Trigger the cleanup transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & cleanup(LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the activate transition. + /* + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & activate(); + /// Trigger the activate transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & activate(LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the deactivate transition + /* + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & deactivate(); + /// Trigger the deactivate transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & deactivate(LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the shutdown transition + /* + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & shutdown(); + /// Trigger the shutdown transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Register the configure callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool register_on_configure(std::function fcn); + /// Register the cleanup callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool register_on_cleanup(std::function fcn); + /// Register the shutdown callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool register_on_shutdown(std::function fcn); + /// Register the activate callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool register_on_activate(std::function fcn); + /// Register the deactivate callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool register_on_deactivate(std::function fcn); + /// Register the error callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool register_on_error(std::function fcn); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index eecbf43fc4..77c1feb682 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -27,18 +27,33 @@ typedef struct rcl_lifecycle_state_t rcl_lifecycle_state_t; namespace rclcpp_lifecycle { +/// Abstract class for the Lifecycle's states. +/** + * There are 4 primary states: Unconfigured, Inactive, Active and Finalized. + */ class State { public: RCLCPP_LIFECYCLE_PUBLIC explicit State(rcutils_allocator_t allocator = rcutils_get_default_allocator()); + /// State constructor. + /** + * \param[in] id of the state + * \param[in] label of the state + * \param[in] allocator a valid allocator used to initialized the state. + */ RCLCPP_LIFECYCLE_PUBLIC State( uint8_t id, const std::string & label, rcutils_allocator_t allocator = rcutils_get_default_allocator()); + /// State constructor. + /** + * \param[in] rcl_lifecycle_state_handle structure with the state details + * \param[in] allocator a valid allocator used to initialized the state. + */ RCLCPP_LIFECYCLE_PUBLIC explicit State( const rcl_lifecycle_state_t * rcl_lifecycle_state_handle, @@ -53,10 +68,18 @@ class State RCLCPP_LIFECYCLE_PUBLIC State & operator=(const State & rhs); + /// Return the id. + /** + * \return id of the state + */ RCLCPP_LIFECYCLE_PUBLIC uint8_t id() const; + /// Return the label. + /** + * \return label of state + */ RCLCPP_LIFECYCLE_PUBLIC std::string label() const; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp index a566a22264..51ca25d2d6 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -28,24 +28,48 @@ typedef struct rcl_lifecycle_transition_t rcl_lifecycle_transition_t; namespace rclcpp_lifecycle { +/// The Transition class abstract the Lifecycle's states. +/** + * There are 7 transitions exposed to a supervisory process, they are: create, configure, + * cleanup, activate, deactivate, shutdown and destroy. + */ class Transition { public: RCLCPP_LIFECYCLE_PUBLIC Transition() = delete; + /// Transition constructor. + /** + * \param[in] id of the transition + * \param[in] label of the transition + * \param[in] allocator a valid allocator used to initialized the state. + */ RCLCPP_LIFECYCLE_PUBLIC explicit Transition( uint8_t id, const std::string & label = "", rcutils_allocator_t allocator = rcutils_get_default_allocator()); + /// Transition constructor. + /** + * \param[in] id of the transition + * \param[in] label of the transition + * \param[in] start state of the transition + * \param[in] goal state of the transition + * \param[in] allocator a valid allocator used to initialized the state. + */ RCLCPP_LIFECYCLE_PUBLIC Transition( uint8_t id, const std::string & label, State && start, State && goal, rcutils_allocator_t allocator = rcutils_get_default_allocator()); + /// Transition constructor. + /** + * \param[in] rcl_lifecycle_transition_handle structure with the transition details + * \param[in] allocator a valid allocator used to initialized the state. + */ RCLCPP_LIFECYCLE_PUBLIC explicit Transition( const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle, @@ -60,18 +84,34 @@ class Transition RCLCPP_LIFECYCLE_PUBLIC Transition & operator=(const Transition & rhs); + /// Return the id. + /** + * \return id of the state + */ RCLCPP_LIFECYCLE_PUBLIC uint8_t id() const; + /// Return the label. + /** + * \return label of the transition + */ RCLCPP_LIFECYCLE_PUBLIC std::string label() const; + /// Return the start state of the transition. + /** + * \return start state of the transition. + */ RCLCPP_LIFECYCLE_PUBLIC State start_state() const; + /// Return the goal state of the transition. + /** + * \return goal state of the transition. + */ RCLCPP_LIFECYCLE_PUBLIC State goal_state() const; From 750754d24d8d80548bbbf8a078b228b4ec97fe3e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 19 May 2020 10:56:36 -0700 Subject: [PATCH 056/121] [rclcpp] API review March 2020 (#1031) * API review part 1 Signed-off-by: William Woodall * update pre second meeting Signed-off-by: William Woodall * notes from 2020-03-23 meeting Signed-off-by: William Woodall * online review comments Signed-off-by: William Woodall --- rclcpp/doc/api_review_march_2020.md | 437 ++++++++++++++++++++++++++++ 1 file changed, 437 insertions(+) create mode 100644 rclcpp/doc/api_review_march_2020.md diff --git a/rclcpp/doc/api_review_march_2020.md b/rclcpp/doc/api_review_march_2020.md new file mode 100644 index 0000000000..49ffa1feb8 --- /dev/null +++ b/rclcpp/doc/api_review_march_2020.md @@ -0,0 +1,437 @@ +# API Review for `rclcpp` from March 2020 + +## Notes + +### Off-Topic Questions + +> [rclcpp_action] There exists a thread-safe and non-thread-safe way to get the goal result from an action client. We probably want to remove the public interface to the non-thread safe call (or consolidate somehow): https://github.com/ros2/rclcpp/issues/955 + +`rclcpp_action` is out of scope atm. + +**Notes from 2020-03-19**: To be handled in separate API review. + +## Architecture + +### Calling Syntax and Keeping Node-like Class APIs in Sync + +> Currently, much of the API is exposed via the `rclcpp::Node` class, and due to the nature of the current architecture there is a lot of repeated code to expose these methods and then call the implementations which are in other classes like `rclcpp::node_interfaces::NodeTopics`, for example. +> +> Also, we have other versions of the class `rclcpp::Node` with different semantics and interfaces, like `rclcpp_lifecycle::LifecycleNode`, and we have been having trouble keeping the interface provided there up to date with how things are done in `rclcpp::Node`. Since `LifecycleNode` has a different API from `Node` in some important cases, it does not just inherit from `Node`. +> +> There are two main proposals (as I see it) to try and address this issue, either (a) break up the functionality in `Node` so that it is in separate classes and make `Node` multiple inherit from those classes, and then `LifecycleNode` could selectively inherit from those as well, or (b) change our calling convention from `node->do_thing(...)` to be `do_thing(node, ...)`. +> +> For (a) which commonly referred to as the [Policy Based Design Pattern](https://en.wikipedia.org/wiki/Modern_C%2B%2B_Design#Policy-based_design), we'd be reversing previous design decisions which we discussed at length where we decided to use composition over inheritance for various reasons. +> One of the reasons was testing, with the theory that having simpler separate interfaces we could more easily mock them as needed for testing. +> The testing goal would still be met, either by keeping the "node_interface" classes as-is or by mocking the classes that node would multiple inherit from, however it's harder to indicate that a function needs a class that multiple inherits from several classes but not others. +> Also having interdependency between the classes which are inherited from is a bit complicated in this design pattern. +> +> For (b), we would be changing how we recommend all code be written (not a trivial thing to do at all), because example code like `auto pub = node->create_publsiher(...);` would be come `auto pub = create_publisher(node, ...);`. +> This has some distinct advantages, however, in that it allows us to write these functions, like `create_publisher(node, ...)`, so that the node argument can be any class that meets the criteria of the function. +> That not only means that when we add a feature it automatically works with `Node` and `LifecycleNode` without adding anything to them, it also means that user defined `Node`-like classes will also work, even if they do not inherit from or provide the complete interface for `rclcpp::Node`. +> Another major downside of this approach is discoverability of the API when using auto-completion in text editors, as `node->` will often give you a list of methods to explore, but with the new calling convention, there's not way to get an auto complete for code who's first argument is a `Node`-like class. +> +> Both of the above approaches address some of the main concerns, which are: keeping `Node` and `LifecycleNode` in sync, reducing the size of the `Node` class so it is more easily maintained, documented, and so that related functions are grouped more clearly. + +- https://github.com/ros2/rclcpp/issues/898 +- https://github.com/ros2/rclcpp/issues/509 +- https://github.com/ros2/rclcpp/issues/855 +- https://github.com/ros2/rclcpp/issues/985 + - subnode feature is in rclcpp::Node only, complicating "node using" API designs +- http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2014/n4174.pdf +- https://en.wikipedia.org/wiki/Uniform_Function_Call_Syntax#C++_proposal + - "Many programmers are tempted to write member functions to get the benefits of the member function syntax (e.g. "dot-autocomplete" to list member functions);[6] however, this leads to excessive coupling between classes.[7]" + +**Suggested Action**: Document the discussion and defer until Foxy. + +**Notes from 2020-03-19**: + +- Another version of (b) could be to have classes that are constructed with node, e.g. `Publisher(node, ...)` rather than `node->create_publisher(...)` +- (tfoote) interface class? `NodeInterface::something(node_like)` + - DRY? + - `NodeInterface::` -> only life cycle node methods +- (karsten) use interface classes directly, e.g. node->get_X_interface()->do_thing() +- (dirk) use macros to add methods to class + - Question: Do we want tab-completable API (specifically list of member functions)? +- Question: Is consistency in calling between core and non-core features more important than tab-completion? +- Add better example of adding new feature and not needing to touch `rclcpp::Node`. +- (dirk) methods and free functions not mutually exclusive. + +### Scoped Versus Non-Scoped Entities (e.g. Publishers/Subscriptions) + +> Currently, Publisher and Subscription (and similar entities) are scoped, meaning that when they are created they are added to the ROS graph as a side effect, and when they are let out of scope they remove themselves from the graph too. +> Additionally, they necessarily have shared state with the system, for instance when you are spinning on a node, the executor shares ownership of the Subscriptions with the user. +> Therefore, the Subscription only gets removed when both the user and executor are done with it. +> +> This shared ownership is accomplished with the use of shared pointers and weak pointers. +> +> There are a few concerns here, (a) use of shared pointers confuses users, (b) overhead of shared pointers and lack of an ability to use these classes on the stack rather than the heap, and (c) complexity of shutdown of an entity from the users perspective. +> +> For (a), some users are overwhelmed by the need to use a shared pointer. +> In ROS 1 this was avoided by having a class which itself just thinly wraps a shared pointer (see: https://github.com/ros/ros_comm/blob/ac9f88c59a676ca6895e13445fc7d71f398ebe1f/clients/roscpp/include/ros/subscriber.h#L108-L111). +> This could be achieved in ROS 2 either by doing the same with a wrapper class (at the expense of lots of repeated code), or by eliminating the need for using shared ownership. +> +> For (b), for some use cases, especially resource constrained / real-time / safety-critical environments, requiring these classes to be on the heap rather than the stack is at least inconvenient. +> Additionally, there is a cost associated with using shared pointers, in the storage of shared state and in some implementation the use of locks or at least atomics for thread-safety. +> +> For (c), this is the most concerning drawback, because right now when a user lets their shared pointer to a, for example, Subscription go out of scope, a post condition is not that the Subscription is destroyed, nor that it has been removed from the graph. +> In stead, the behavior is more like "at some point in the future the Subscription will be destroyed and removed from the graph, when the system is done with it". +> This isn't a very satisfactory contract, as some users may wish to know when the Subscription has been deleted, but cannot easily know that. +> +> The benefit to the shared state is a safety net for users. +> The alternative would be to document that a Subscription, again for example, cannot be deleted until the system is done with it. +> We'd basically be pushing the responsibility onto the user to ensure the shared ownership is handled properly by the execution of their application, i.e. they create the Subscription, share a reference with the system (adding it by reference to an executor, for example), and they have to make sure the system is done with it before deleting the Subscription. +> +>Separately, from the above points, there is the related concern of forcing the user to keep a copy of their entities in scope, whether it be with a shared pointer or a class wrapping one. +> There is the desire to create it and forget it in some cases. +> The downside to this is that if/when the user wants to destroy the entity, they have no way of doing that as they have no handle or unique way to address the entity. +> +> One proposed solution would be to have a set of "named X" APIs, e.g. `create_named_subscription` rather than just `create_subscription`. +> This would allow the user to address the Subscription in the future in order to obtain a new reference to it or delete it. + +- https://github.com/ros2/rclcpp/issues/506 +- https://github.com/ros2/rclcpp/issues/726 + +**Suggested Action**: Consolidate to a single issue, and defer. + +**Notes from 2020-03-23**: + +- (chris) Putting ownership mechanics on user is hard. +- (dirk) add documentation clearly outlining ownership +- (shane) warn on unused to catch issues with immediately deleted items +- (tfoote) debugging output for destruction so it easy to see when reviewing logs +- (chris) possible to create API that checks for destruction + - (william) might lead to complex synchronization issues +- (tfoote) could add helper classes to make scoped things non-scoped + - (shane) concerned that there is no longer "one good way" to do it + +### Allow QoS to be configured externally, like we allow remapping of topic names + +> Suggestion from @stonier: allow the qos setting on a topic to be changed externally at startup, similar to how we do topic remapping (e.g., do it on the command-line using appropriate syntax). +> +> To keep the syntax manageable, we might just allow profiles to be picked. + +- https://github.com/ros2/rclcpp/issues/239 + +**Suggested Action**: Update issue, defer for now. + +**Notes from 2020-03-19**: + +- (wjwwood) it depends on the QoS setting, but many don't make sense, mostly because they can change some of the behaviors of underlying API +- (dirk) Should developers expose a parameter instead? +- (multiple) should be a feature that makes configuring them (after opt-in) consistent +- (jacob) customers feedback was that this was expected, surprised it was not allowed +- (karsten) could limit to profiles + +## Init/shutdown and Context + +### Consider renaming `rclcpp::ok()` + +> Old discussion to rename `rclcpp::ok()` to something more specific, like `rclcpp::is_not_shutdown()` or the corollary `rclcpp::is_shutdown()`. + +- https://github.com/ros2/rclcpp/issues/3 + +**Suggested Action**: Defer. + +**Notes from 2020-03-19**: + +- (shane) preference to not have a negative in the function name + +## Executor + +### Exposing Scheduling of Tasks in Executor and a Better Default + +> Currently there is a hard coded procedure for handling ready tasks in the executor, first timers, then subscriptions, and so on. +> This scheduling is not fair and results in non-deterministic behavior and starvation issues. +> +> We should provide a better default scheduling which is fairer and ideally deterministic, something like round-robin or FIFO. +> +> Additionally, we should make it easier to let the user override the scheduling logic in the executor. + +- https://github.com/ros2/rclcpp/pull/614 +- https://github.com/ros2/rclcpp/issues/633 +- https://github.com/ros2/rclcpp/issues/392 + +**Suggested Action**: Follow up on proposals to implement FIFO scheduling and refactor the Executor design to more easily expose the scheduling logic. + +**Notes from 2020-03-19**: + +- No comments. + +### Make it possible to wait on entities (e.g. Subscriptions) without an Executor + +> Currently, it is only possible to use things like Timers and Subscriptions and Services with an executor. +> It should be possible, however, to either poll these entities or wait on them and then decide which to process as a user. +> +> This is most easily accomplished with a WaitSet-like class. + +- https://github.com/ros2/rclcpp/issues/520 + +**Suggested Action**: implement WaitSet class in rclcpp so that this is possible, and make "waitable" entities such that they can be polled, e.g. `Subscription`s should have a user facing `take()` method, which can fail if no data is available. + +**Notes from 2020-03-19**: + +- No comments. + +### Make it possible to use multiple executors per node + +> Currently, you cannot use more than one executor per node, this limits your options when it comes to distributing work within a node across threads. +> You can use a multi-threaded executor, or make your own executor which does this, but it is often convenient to be able to spin part of the node separately from the the rest of the node. + +- https://github.com/ros2/rclcpp/issues/519 + +**Suggested Action**: Make this possible, moving the exclusivity to be between an executor and callback groups rather than nodes. + +**Notes from 2020-03-19**: + +- No comments. + +### Implement a Lock-free Executor + +> This would presumably be useful for real-time and safety critical systems where locks and any kind of blocking code is considered undesirable. + +- https://github.com/ros2/rclcpp/issues/77 + +**Suggested Action**: Keep in backlog until someone needs it specifically. + +**Notes from 2020-03-19**: + +- No comments. + +### Add implementation of `spin_some()` to the `MultiThreadedExecutor` + +> Currently `spin_some()` is only available in the `SingleThreadedExecutor`. + +- https://github.com/ros2/rclcpp/issues/85 + +**Suggested Action**: Defer. + +**Notes from 2020-03-19**: + +- No comments. + +## Node + +### Do argument parsing outside of node constructor + +> Things that come from command line arguments should be separately passed into the node's constructor rather than passing in arguments and asking the node to do the parsing. + +- https://github.com/ros2/rclcpp/issues/492 + +**Suggested Action**: Defer until after foxy. + +**Notes from 2020-03-23**: + +- (dirk) may be related to ROS 1 heritage of argc/argv being passed to node directly +- (shane) impacts rcl API as well, two parts "global options" as well node specific options +- (dirk) what is the recommendation to users that want to add arguments programmatically + - user should be able to get non-ros argc/argv somehow (seems like you can now) +- (jacob) the argument in NodeOptions are used for application specific argument via component loading as well + +## Timer + +### Timer based on ROS Time + +> `node->create_wall_timer` does exactly what it says; creates a timer that will call the callback when the wall time expires. But this is almost never what the user wants, since this won’t work properly in simulation. Suggestion: deprecate `create_wall_timer`, add a new method called `create_timer` that takes the timer to use as one of the arguments, which defaults to ROS_TIME. + +- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/include/rclcpp/node.hpp#L219-L230 +- https://github.com/ros2/rclcpp/issues/465 + +**Suggested Action**: Promote `rclcpp::create_timer()` which is templated on a clock type, as suggested, but leave `create_wall_timer` as a convenience. + +**Notes from 2020-03-19**: + +- (shane) may be a `rclcpp::create_timer()` that can be used to create a non-wall timer already + +## Publisher + +## Subscription + +### Callback Signature + +> Is there a reason the subscription callback must have a smart pointer argument instead of accepting a const-reference argument? + +- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/include/rclcpp/any_subscription_callback.hpp#L44-L52 +- https://github.com/ros2/rclcpp/issues/281 + +**Suggested Action**: Provide const reference as an option, add documentation as to the implications of one callback signature versus others. + +**Notes from 2020-03-19**: + +- (dirk) have const reference and document it + +## Service Server + +### Allow for asynchronous Service Server callbacks + +> Currently, the only callback signature for Service Servers takes a request and must return a response. +> This means that all of the activity of the service server has to happen within that function. +> This can cause issues, specifically if you want to call another service within the current service server's callback, as it causes deadlock issues trying to synchronously call the second service within a spin callback. +> More generally, it seems likely that long running service server callbacks may be necessary in the future and requiring them to be synchronous would tie up at least on thread in the spinning executor unnecessarily. + +- https://github.com/ros2/rclcpp/issues/491 + +**Suggested Action**: Defer. + +**Notes from 2020-03-23**: + +- (dirk) likely new API, so possible to backport + +## Service Client + +### Callback has SharedFuture rather than const reference to response + +> Why does the Client::send_async_request take in a callback that has a SharedFuture argument instead of an argument that is simply a const-reference (or smart pointer) to the service response type? The current API seems to imply that the callback ought to check whether the promise is broken or fulfilled before trying to access it. Is that the case? If so, it should be documented in the header. + +- https://github.com/ros2/rclcpp/blob/7c1721a0b390be8242a6b824489d0bc861f6a0ad/rclcpp/include/rclcpp/client.hpp#L134 + +**Suggested Action**: Update ticket and defer. + +**Notes from 2020-03-19**: + +- (wjwwood) we wanted the user to handle error cases with the future? +- (dirk) future allows for single callback (rather than one for response and one for error) +- (jacob) actions uses a "wrapped result" object + +### rclcpp missing synchronous `send_request` and issues with deadlocks + +> This has been reported by several users, but there is only an `async_send_request` currently. `rclpy` has a synchronous `send_request` but it has issues with deadlock, specifically if you call it without spinning in another thread then it will deadlock. Or if you call it from within a spin callback when using a single threaded executor, it will deadlock. + +- https://discourse.ros.org/t/synchronous-request-to-service-in-callback-results-in-deadlock/12767 +- https://github.com/ros2/rclcpp/issues/975 +- https://github.com/ros2/demos/blob/948b4f4869298f39cfe99d3ae517ad60a72a8909/demo_nodes_cpp/src/services/add_two_ints_client.cpp#L24-L39 + +**Suggested Action**: Update issue and defer. Also defer decision on reconciling rclpy's send_request. + +**Notes from 2020-03-23**: + +- (karsten/shane) async spinner helps in rclpy version, rclcpp could emulate +- (chris) sees three options: + - only async (current case in rclcpp) + - have sync version, add lots of docs that spinning needs to happen elsewhere (current case for rclpy) + - reentrant spinning +- (william) you either need async/await from language or ".then" syntax (we have this in async_send_request()) +- (chris) more error checking for recursive spinning +- (chris) weird that rclcpp and rclpy have different API +- (shane) thinks it is ok to have different API, but rclpy is not ideal + +## Parameters + +### Expected vs Unexpected parameters + +> Allow node author to define expected parameters and what happens when an unexpected parameter is set. + +- https://github.com/ros2/rclcpp/issues/475 +- https://github.com/ros2/rclcpp/tree/check_parameters + +**Suggested Action**: Defer as nice to have. + +**Notes from 2020-03-23**: + +- None. + +### Implicitly cast integer values for double parameters + +> If we try to pass an integer value to a double parameter from the command line or from a parameters YAML file we get a `rclcpp::ParameterTypeException`. +> For example, passing a parameter from the command line: +> +> ros2 run foo_package foo_node --ros-args -p foo_arg:=1 +> +> results in the following error: +> +> terminate called after throwing an instance of 'rclcpp::ParameterTypeException' +> what(): expected [double] got [integer] +> +> and we can fix it by explicitly making our value a floating point number: +> +> ros2 run foo_package foo_node --ros-args -p foo_arg:=1.0 +> +> But, it seems reasonable to me that if a user forgets to explicitly provide a floating point value that we should implicitly cast an integer to a float (as is done in many programming languages). + +- https://github.com/ros2/rclcpp/issues/979 + +**Suggested Action**: Continue with issue. + +**Notes from 2020-03-23**: + +- (shane) says "yes please" :) + +### Use `std::variant` instead of custom `ParameterValue` class + +> This is only possible if C++17 is available, but it would simplify our code, make our interface more standard, and allow us to use constexpr-if to simply our templated code. + +**Suggested Action**: Create an issue for future work. + +**Notes from 2020-03-23**: + +- (chris) not sure churn is worth +- (ivan) other places for std::variant, like AnySubscriptionCallback + +### Cannot set name or value on `Parameter`/`ParameterValue` + +> Both `Parameter` and `ParameterValue` are read-only after construction. + +- https://github.com/ros2/rclcpp/issues/238 + +**Suggested Action**: Update issue, possibly close. + +**Notes from 2020-03-23**: + +- (chris/william) setting values on temporary (local) objects is not reflected in the node, so misleading + +## Parameter Clients + +### No timeout option with synchronous parameter client calls + +> As an example, SyncParametersClient::set_parameters doesn't take a timeout option. So, if anything goes wrong in the service call (e.g. the server goes down), we will get stuck waiting indefinitely. + +- https://github.com/ros2/rclcpp/issues/360 +- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/src/rclcpp/parameter_client.cpp#L453-L468 + +**Suggested Action**: Update issue, decide if it can be taken for Foxy or not. + +**Notes from 2020-03-23**: + +- (tfoote) Seems like adding a timeout is a good idea. + +### Name of AsyncParametersClient inconsistent + +> AsyncParameter**s**Client uses plural, when filename is singular (and ParameterService is singular): + +- https://github.com/ros2/rclcpp/blob/7c1721a0b390be8242a6b824489d0bc861f6a0ad/rclcpp/include/rclcpp/parameter_client.hpp#L44 + +**Suggested Action**: Reconcile class and file name, switch to singular name? + +**Notes from on-line, post 2020-03-23 meeting**: + +- (tfoote) +1 for homogenizing to singular + +### `SyncParametersClient::get_parameters` doesn't allow you to detect error cases + +> E.g. https://github.com/ros2/rclcpp/blob/249b7d80d8f677edcda05052f598de84f4c2181c/rclcpp/src/rclcpp/parameter_client.cpp#L246-L257 returns an empty vector if something goes wrong which is also a valid response. + +- https://github.com/ros2/rclcpp/issues/200 +- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/src/rclcpp/parameter_client.cpp#L412-L426 + +**Suggested Action**: Throw an exception to indicate if something went wrong and document other expected conditions of the API. + +**Notes from on-line, post 2020-03-23 meeting**: + +- (tfoote) An empty list is not a valid response unless you passed in an empty list. The return should have the same length as the request in the same order. Any parameters that are not set should return a ParameterVariant with type PARAMETER_NOT_SET. to indicate that it was polled and determined to not be set. Suggested action improve documentation of the API to clarify a short or incomplete. +- (jacobperron) I think throwing an exception is also a valid action, making it clear that an error occurred. +- (wjwwood) Using exceptions to indicate an exceptional case (something went wrong) seems reasonable to me too. + +## Clock + +### Clock Jump callbacks on System or Steady time? + +> Currently time jump callbacks are registered via Clock::create_jump_handler(). Jump handlers are only invoked by TimeSource::set_clock(). This is only called if the clock type is RCL_ROS_TIME and ROS time is active. + +- https://github.com/ros2/rclcpp/issues/528 + +**Suggested Action**: Document that time jumping is only detected with ROS time, consider a warning. + +**Notes from on-line, post 2020-03-23 meeting**: + +- (tfoote) There should be no jumps in steady time. If there's a big change in system time, it doesn't necessarily mean that time jumped, just that you might have been sleeping for a long time. Most ntp systems adjust the slew rate these days instead of jumping but still that's an external process and I don't know of any APIs to introspect the state of the clock. I'm not sure that we have a way to detect jumps in time for system or steady time. To that end I think that we should be clear that we only provide callbacks when simulation time starts or stops, or simulation time jumps. We should also strongly recommend that operators not actively adjust their system clocks while running ROS nodes. +- (jacobperron) I agree with Tully, if we don't have a way to detect system time jumps then I think we should just document that this only works with ROS time. In addition to documentation, we could log an info or warning message if the user registers jump callback with steady or system time, but it may be unnecessarily noisy. + From 375de01f446f9a5efb2a79ed6028d1ff977ccb67 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 21 May 2020 11:39:07 -0700 Subject: [PATCH 057/121] Deprecate ClientGoalHandle::async_result() (#1120) Fixes https://github.com/ros2/rclcpp/issues/955 There are currently two public APIs for users to get the result of a goal. This change deprecates one of the APIs, which was considered to be unsafe as it may result in a race with user-code and raise an exception. Signed-off-by: Jacob Perron --- rclcpp_action/include/rclcpp_action/client.hpp | 2 +- .../include/rclcpp_action/client_goal_handle.hpp | 16 ++++++++++++++++ .../rclcpp_action/client_goal_handle_impl.hpp | 7 +++++++ rclcpp_action/test/test_client.cpp | 1 - 4 files changed, 24 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 7133e8ce14..27ced3dbdb 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -411,7 +411,7 @@ class Client : public ClientBase goal_handle->set_result_callback(result_callback); } this->make_result_aware(goal_handle); - return goal_handle->async_result(); + return goal_handle->async_get_result(); } /// Asynchronously request a goal be canceled. diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 42e2844c51..9ad8cb266a 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -91,6 +91,8 @@ class ClientGoalHandle /// Get a future to the goal result. /** + * \deprecated Use rclcpp_action::Client::async_get_result() instead. + * * This method should not be called if the `ignore_result` flag was set when * sending the original goal request (see Client::async_send_goal). * @@ -99,6 +101,7 @@ class ClientGoalHandle * \throws exceptions::UnawareGoalHandleError If the the goal handle is unaware of the result. * \return A future to the result. */ + [[deprecated("use rclcpp_action::Client::async_get_result() instead")]] std::shared_future async_result(); @@ -134,6 +137,19 @@ class ClientGoalHandle typename ClientGoalHandle::SharedPtr shared_this, typename std::shared_ptr feedback_message); + /// Get a future to the goal result. + /** + * This method should not be called if the `ignore_result` flag was set when + * sending the original goal request (see Client::async_send_goal). + * + * `is_result_aware()` can be used to check if it is safe to call this method. + * + * \throws exceptions::UnawareGoalHandleError If the the goal handle is unaware of the result. + * \return A future to the result. + */ + std::shared_future + async_get_result(); + /// Returns the previous value of awareness bool set_result_awareness(bool awareness); 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..277e2576a0 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -58,6 +58,13 @@ ClientGoalHandle::get_goal_stamp() const template std::shared_future::WrappedResult> ClientGoalHandle::async_result() +{ + return this->async_get_result(); +} + +template +std::shared_future::WrappedResult> +ClientGoalHandle::async_get_result() { std::lock_guard guard(handle_mutex_); if (!is_result_aware_) { diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index e89b19a470..8735717003 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -304,7 +304,6 @@ TEST_F(TestClient, async_send_goal_no_callbacks) EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_FALSE(goal_handle->is_feedback_aware()); EXPECT_FALSE(goal_handle->is_result_aware()); - EXPECT_THROW(goal_handle->async_result(), rclcpp_action::exceptions::UnawareGoalHandleError); } TEST_F(TestClient, async_send_goal_no_callbacks_wait_for_result) From 90c71e65487f55584e32de84f8533a8e59dc7bb5 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Fri, 22 May 2020 11:36:32 -0700 Subject: [PATCH 058/121] remove empty lines within method signatures (#1128) Signed-off-by: Dirk Thomas --- .../include/rclcpp/node_interfaces/node_base.hpp | 16 ---------------- .../rclcpp/node_interfaces/node_graph.hpp | 12 ------------ .../rclcpp/node_interfaces/node_logging.hpp | 2 -- .../rclcpp/node_interfaces/node_services.hpp | 2 -- .../rclcpp/node_interfaces/node_timers.hpp | 1 - .../rclcpp/node_interfaces/node_waitables.hpp | 2 -- 6 files changed, 35 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 0e124f6f56..4386ee5e55 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -50,82 +50,66 @@ class NodeBase : public NodeBaseInterface ~NodeBase(); RCLCPP_PUBLIC - const char * get_name() const override; RCLCPP_PUBLIC - const char * get_namespace() const override; RCLCPP_PUBLIC - const char * get_fully_qualified_name() const override; RCLCPP_PUBLIC - rclcpp::Context::SharedPtr get_context() override; RCLCPP_PUBLIC - rcl_node_t * get_rcl_node_handle() override; RCLCPP_PUBLIC - const rcl_node_t * get_rcl_node_handle() const override; RCLCPP_PUBLIC - std::shared_ptr get_shared_rcl_node_handle() override; RCLCPP_PUBLIC - std::shared_ptr get_shared_rcl_node_handle() const override; RCLCPP_PUBLIC - rclcpp::CallbackGroup::SharedPtr create_callback_group(rclcpp::CallbackGroupType group_type) override; RCLCPP_PUBLIC - rclcpp::CallbackGroup::SharedPtr get_default_callback_group() override; RCLCPP_PUBLIC - bool callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) override; RCLCPP_PUBLIC - const std::vector & get_callback_groups() const override; RCLCPP_PUBLIC - std::atomic_bool & get_associated_with_executor_atomic() override; RCLCPP_PUBLIC - rcl_guard_condition_t * get_notify_guard_condition() override; RCLCPP_PUBLIC - std::unique_lock acquire_notify_guard_condition_lock() const override; RCLCPP_PUBLIC - bool get_use_intra_process_default() const override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index 26d91aecfd..5882c727af 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -57,64 +57,52 @@ class NodeGraph : public NodeGraphInterface ~NodeGraph(); RCLCPP_PUBLIC - std::map> get_topic_names_and_types(bool no_demangle = false) const override; RCLCPP_PUBLIC - std::map> get_service_names_and_types() const override; RCLCPP_PUBLIC - std::vector get_node_names() const override; RCLCPP_PUBLIC - std::vector> get_node_names_and_namespaces() const override; RCLCPP_PUBLIC - size_t count_publishers(const std::string & topic_name) const override; RCLCPP_PUBLIC - size_t count_subscribers(const std::string & topic_name) const override; RCLCPP_PUBLIC - const rcl_guard_condition_t * get_graph_guard_condition() const override; RCLCPP_PUBLIC - void notify_graph_change() override; RCLCPP_PUBLIC - void notify_shutdown() override; RCLCPP_PUBLIC - rclcpp::Event::SharedPtr get_graph_event() override; RCLCPP_PUBLIC - void wait_for_graph_change( rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout) override; RCLCPP_PUBLIC - size_t count_graph_users() override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp index 17f835b0dd..ea91064be7 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp @@ -42,12 +42,10 @@ class NodeLogging : public NodeLoggingInterface ~NodeLogging(); RCLCPP_PUBLIC - rclcpp::Logger get_logger() const override; RCLCPP_PUBLIC - const char * get_logger_name() const override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp index 01c9912344..1eecf69eee 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp @@ -42,14 +42,12 @@ class NodeServices : public NodeServicesInterface ~NodeServices(); RCLCPP_PUBLIC - void add_client( rclcpp::ClientBase::SharedPtr client_base_ptr, rclcpp::CallbackGroup::SharedPtr group) override; RCLCPP_PUBLIC - void add_service( rclcpp::ServiceBase::SharedPtr service_base_ptr, diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp index 03f151ff25..9f26721d4b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp @@ -42,7 +42,6 @@ class NodeTimers : public NodeTimersInterface /// Add a timer to the node. RCLCPP_PUBLIC - void add_timer( rclcpp::TimerBase::SharedPtr timer, diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp index fd560bca48..46c24d9aa1 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp @@ -41,14 +41,12 @@ class NodeWaitables : public NodeWaitablesInterface ~NodeWaitables(); RCLCPP_PUBLIC - void add_waitable( rclcpp::Waitable::SharedPtr waitable_base_ptr, rclcpp::CallbackGroup::SharedPtr group) override; RCLCPP_PUBLIC - void remove_waitable( rclcpp::Waitable::SharedPtr waitable_ptr, From b1b525b1c6f1b298f5858352008622df0c8bf938 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 22 May 2020 11:41:34 -0700 Subject: [PATCH 059/121] [rclcpp_action] Action client holds weak pointers to goal handles (#1122) * [rclcpp_action] Action client holds weak pointers to goal handles Fixes #861 It is against the design of ROS actions to rely on the status topic for the core implementation, instead it should just be used for introspection. Rather than relying on the status topic to remove references to goal handles, the action client instead holds weak pointers to the goal handles. This way as long as a user holds a reference to the goal handle they can use it to interact with the action client. Signed-off-by: Jacob Perron * Move cleanup logic to the end of the function Signed-off-by: Jacob Perron * Add TODO Signed-off-by: Jacob Perron * Log debug messages when dropping a weak references to goal handles Signed-off-by: Jacob Perron * Improve documentation Signed-off-by: Jacob Perron --- .../include/rclcpp_action/client.hpp | 57 +++++++++++++++---- 1 file changed, 45 insertions(+), 12 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 27ced3dbdb..71b0c866a0 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -331,7 +331,9 @@ class Client : public ClientBase * If the goal is accepted by an action server, the returned future is set to a `ClientGoalHandle`. * If the goal is rejected by an action server, then the future is set to a `nullptr`. * - * The goal handle is used to monitor the status of the goal and get the final result. + * The returned goal handle is used to monitor the status of the goal and get the final result. + * It is valid as long as you hold a reference to the shared pointer or until the + * rclcpp_action::Client is destroyed at which point the goal status will become UNKNOWN. * * \param[in] goal The goal request. * \param[in] options Options for sending the goal request. Contains references to callbacks for @@ -386,6 +388,26 @@ class Client : public ClientBase } } }); + + // TODO(jacobperron): Encapsulate into it's own function and + // consider exposing an option to disable this cleanup + // To prevent the list from growing out of control, forget about any goals + // with no more user references + { + std::lock_guard guard(goal_handles_mutex_); + auto goal_handle_it = goal_handles_.begin(); + while (goal_handle_it != goal_handles_.end()) { + if (!goal_handle_it->second.lock()) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during send_goal()"); + goal_handle_it = goal_handles_.erase(goal_handle_it); + } else { + ++goal_handle_it; + } + } + } + return future; } @@ -494,7 +516,10 @@ class Client : public ClientBase std::lock_guard guard(goal_handles_mutex_); auto it = goal_handles_.begin(); while (it != goal_handles_.end()) { - it->second->invalidate(); + typename GoalHandle::SharedPtr goal_handle = it->second.lock(); + if (goal_handle) { + goal_handle->invalidate(); + } it = goal_handles_.erase(it); } } @@ -546,7 +571,15 @@ class Client : public ClientBase "Received feedback for unknown goal. Ignoring..."); return; } - typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; + typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock(); + // Forget about the goal if there are no more user references + if (!goal_handle) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during feedback callback"); + goal_handles_.erase(goal_id); + return; + } auto feedback = std::make_shared(); *feedback = feedback_message->feedback; goal_handle->call_feedback_callback(goal_handle, feedback); @@ -575,16 +608,16 @@ class Client : public ClientBase "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) - { + typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock(); + // Forget about the goal if there are no more user references + if (!goal_handle) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during status callback"); goal_handles_.erase(goal_id); + continue; } + goal_handle->set_status(status.status); } } @@ -639,7 +672,7 @@ class Client : public ClientBase return future; } - std::map goal_handles_; + std::map goal_handles_; std::mutex goal_handles_mutex_; }; } // namespace rclcpp_action From 55854a7d35857e6867fc0036ccfb01c379f49749 Mon Sep 17 00:00:00 2001 From: ChenYing Kuo Date: Sat, 23 May 2020 04:14:48 +0800 Subject: [PATCH 060/121] Make sure rmw_publisher_options is initialized in to_rcl_publisher_options. (#1099) Signed-off-by: ChenYing Kuo --- rclcpp/include/rclcpp/publisher_options.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index dd24718fd7..9547b349dd 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -71,7 +71,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase rcl_publisher_options_t to_rcl_publisher_options(const rclcpp::QoS & qos) const { - rcl_publisher_options_t result; + rcl_publisher_options_t result = rcl_publisher_get_default_options(); using AllocatorTraits = std::allocator_traits; using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc; auto message_alloc = std::make_shared(*this->get_allocator().get()); From db772b5138d41a48d972ee8a8bb72a91b449b0cb Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 22 May 2020 18:17:01 -0300 Subject: [PATCH 061/121] Fix thread safety issues related to logging (#1125) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/context.hpp | 4 +- rclcpp/src/rclcpp/context.cpp | 65 ++++++++++++------- rclcpp/src/rclcpp/logging_mutex.cpp | 28 ++++++++ rclcpp/src/rclcpp/logging_mutex.hpp | 39 +++++++++++ .../src/rclcpp/node_interfaces/node_base.cpp | 23 +++++-- 6 files changed, 131 insertions(+), 29 deletions(-) create mode 100644 rclcpp/src/rclcpp/logging_mutex.cpp create mode 100644 rclcpp/src/rclcpp/logging_mutex.hpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 7e5702cd79..171e0f875d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -54,6 +54,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/init_options.cpp src/rclcpp/intra_process_manager.cpp src/rclcpp/logger.cpp + src/rclcpp/logging_mutex.cpp src/rclcpp/memory_strategies.cpp src/rclcpp/memory_strategy.cpp src/rclcpp/message_info.cpp diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 5d7a6bcecf..61eb27ac88 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -338,8 +338,8 @@ class Context : public std::enable_shared_from_this rclcpp::InitOptions init_options_; std::string shutdown_reason_; - // Keep shared ownership of global logging configure mutex. - std::shared_ptr logging_configure_mutex_; + // Keep shared ownership of the global logging mutex. + std::shared_ptr logging_mutex_; std::unordered_map> sub_contexts_; // This mutex is recursive so that the constructor of a sub context may diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 1918a77a02..ea1c3ec95a 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -1,4 +1,4 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. +// Copyright 2015-2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,27 +23,24 @@ #include "rcl/init.h" #include "rcl/logging.h" + #include "rclcpp/detail/utilities.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/logging.hpp" + +#include "rcutils/error_handling.h" +#include "rcutils/macros.h" + #include "rmw/impl/cpp/demangle.hpp" +#include "./logging_mutex.hpp" + /// Mutex to protect initialized contexts. static std::mutex g_contexts_mutex; /// Weak list of context to be shutdown by the signal handler. static std::vector> g_contexts; -using rclcpp::Context; - -static -std::shared_ptr -get_global_logging_configure_mutex() -{ - static auto mutex = std::make_shared(); - return mutex; -} - -static +/// Count of contexts that wanted to initialize the logging system. size_t & get_logging_reference_count() { @@ -51,10 +48,36 @@ get_logging_reference_count() return ref_count; } +using rclcpp::Context; + +extern "C" +{ +static +void +rclcpp_logging_output_handler( + const rcutils_log_location_t * location, + int severity, const char * name, rcutils_time_point_value_t timestamp, + const char * format, va_list * args) +{ + try { + std::shared_ptr logging_mutex; + logging_mutex = get_global_logging_mutex(); + std::lock_guard guard(*logging_mutex); + return rcl_logging_multiple_output_handler( + location, severity, name, timestamp, format, args); + } catch (std::exception & ex) { + RCUTILS_SAFE_FWRITE_TO_STDERR(ex.what()); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } catch (...) { + RCUTILS_SAFE_FWRITE_TO_STDERR("failed to take global rclcpp logging mutex\n"); + } +} +} // extern "C" + Context::Context() : rcl_context_(nullptr), shutdown_reason_(""), - logging_configure_mutex_(nullptr) + logging_mutex_(nullptr) {} Context::~Context() @@ -116,16 +139,14 @@ Context::init( } if (init_options.auto_initialize_logging()) { - logging_configure_mutex_ = get_global_logging_configure_mutex(); - if (!logging_configure_mutex_) { - throw std::runtime_error("global logging configure mutex is 'nullptr'"); - } - std::lock_guard guard(*logging_configure_mutex_); + logging_mutex_ = get_global_logging_mutex(); + std::lock_guard guard(*logging_mutex_); size_t & count = get_logging_reference_count(); if (0u == count) { - ret = rcl_logging_configure( + ret = rcl_logging_configure_with_output_handler( &rcl_context_->global_arguments, - rcl_init_options_get_allocator(init_options_.get_rcl_init_options())); + rcl_init_options_get_allocator(init_options_.get_rcl_init_options()), + rclcpp_logging_output_handler); if (RCL_RET_OK != ret) { rcl_context_.reset(); rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging"); @@ -228,9 +249,9 @@ Context::shutdown(const std::string & reason) } } // shutdown logger - if (logging_configure_mutex_) { + if (logging_mutex_) { // logging was initialized by this context - std::lock_guard guard(*logging_configure_mutex_); + std::lock_guard guard(*logging_mutex_); size_t & count = get_logging_reference_count(); if (0u == --count) { rcl_ret_t rcl_ret = rcl_logging_fini(); diff --git a/rclcpp/src/rclcpp/logging_mutex.cpp b/rclcpp/src/rclcpp/logging_mutex.cpp new file mode 100644 index 0000000000..5d189c4918 --- /dev/null +++ b/rclcpp/src/rclcpp/logging_mutex.cpp @@ -0,0 +1,28 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rcutils/macros.h" + +std::shared_ptr +get_global_logging_mutex() +{ + static auto mutex = std::make_shared(); + if (RCUTILS_UNLIKELY(!mutex)) { + throw std::runtime_error("rclcpp global logging mutex is a nullptr"); + } + return mutex; +} diff --git a/rclcpp/src/rclcpp/logging_mutex.hpp b/rclcpp/src/rclcpp/logging_mutex.hpp new file mode 100644 index 0000000000..0e7be39237 --- /dev/null +++ b/rclcpp/src/rclcpp/logging_mutex.hpp @@ -0,0 +1,39 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__LOGGING_MUTEX_HPP_ +#define RCLCPP__LOGGING_MUTEX_HPP_ + +#include +#include + +#include "rclcpp/visibility_control.hpp" + +/// Global logging mutex +/** + * This mutex is locked in the following situations: + * - In initialization/destruction of contexts. + * - In initialization/destruction of nodes. + * - In the rcl logging output handler installed by rclcpp, + * i.e.: in all calls to the logger macros, including RCUTILS_* ones. + */ +// Implementation detail: +// A shared pointer to the mutex is used, so that objects that need to use +// it at destruction time can hold it alive. +// In that way, a destruction ordering problem between static objects is avoided. +RCLCPP_LOCAL +std::shared_ptr +get_global_logging_mutex(); + +#endif // RCLCPP__LOGGING_MUTEX_HPP_ diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index f055abaa25..d8c13bd826 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -25,6 +25,8 @@ #include "rmw/validate_namespace.h" #include "rmw/validate_node_name.h" +#include "../logging_mutex.hpp" + using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::node_interfaces::NodeBase; @@ -65,10 +67,17 @@ NodeBase::NodeBase( // 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); + std::shared_ptr logging_mutex = get_global_logging_mutex(); + { + std::lock_guard guard(*logging_mutex); + // TODO(ivanpauno): Instead of mutually excluding rcl_node_init with the global logger mutex, + // rcl_logging_rosout_init_publisher_for_node could be decoupled from there and be called + // here directly. + ret = rcl_node_init( + rcl_node.get(), + 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(); @@ -123,7 +132,11 @@ NodeBase::NodeBase( node_handle_.reset( rcl_node.release(), - [](rcl_node_t * node) -> void { + [logging_mutex](rcl_node_t * node) -> void { + std::lock_guard guard(*logging_mutex); + // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex, + // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called + // here directly. if (rcl_node_fini(node) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", From 11cb640844b3fc32b54b1b9e5d18ce79ecfa1745 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 26 May 2020 10:26:51 -0700 Subject: [PATCH 062/121] expose get_service_names_and_types_by_node from rcl in rclcpp (#1131) * expose get_service_names_and_types_by_node from rcl in rclcpp Signed-off-by: Dirk Thomas * fix spelling Signed-off-by: Dirk Thomas * zero initialize Signed-off-by: Dirk Thomas * check return value and cleanup Signed-off-by: Dirk Thomas * use throw_from_rcl_error Signed-off-by: Dirk Thomas * cleanup error handling Signed-off-by: Dirk Thomas --- rclcpp/include/rclcpp/node.hpp | 6 +++ .../rclcpp/node_interfaces/node_graph.hpp | 6 +++ .../node_interfaces/node_graph_interface.hpp | 14 ++++++ rclcpp/src/rclcpp/node.cpp | 9 ++++ .../src/rclcpp/node_interfaces/node_graph.cpp | 48 ++++++++++++++++++- .../rclcpp_lifecycle/lifecycle_node.hpp | 13 +++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 9 ++++ 7 files changed, 103 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index d54ea21c5e..b73b62d720 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -872,6 +872,12 @@ class Node : public std::enable_shared_from_this std::map> get_service_names_and_types() const; + RCLCPP_PUBLIC + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const; + RCLCPP_PUBLIC size_t count_publishers(const std::string & topic_name) const; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index 5882c727af..538958fc50 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -64,6 +64,12 @@ class NodeGraph : public NodeGraphInterface std::map> get_service_names_and_types() const override; + RCLCPP_PUBLIC + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const override; + RCLCPP_PUBLIC std::vector get_node_names() const override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 26ab3118ef..34d864fe21 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -165,6 +165,20 @@ class NodeGraphInterface std::map> get_service_names_and_types() const = 0; + /// Return a map of existing service names to list of service types for a specific node. + /** + * This function only considers services - not clients. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + */ + RCLCPP_PUBLIC + virtual + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const = 0; + /// Return a vector of existing node names (string). RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 30804809a4..b1f9e66f6a 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -352,6 +352,15 @@ Node::get_service_names_and_types() const return node_graph_->get_service_names_and_types(); } +std::map> +Node::get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const +{ + return node_graph_->get_service_names_and_types_by_node( + node_name, namespace_); +} + size_t Node::count_publishers(const std::string & topic_name) const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 2e4d73440b..0aa361e38d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -70,8 +70,9 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const 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_reset_error(); } - throw std::runtime_error(error_msg + rcl_get_error_string().str); + throw std::runtime_error(error_msg); } std::map> topics_and_types; @@ -111,8 +112,9 @@ NodeGraph::get_service_names_and_types() const error_msg += std::string(", failed also to cleanup service names and types, leaking memory: ") + rcl_get_error_string().str; + rcl_reset_error(); } - throw std::runtime_error(error_msg + rcl_get_error_string().str); + throw std::runtime_error(error_msg); } std::map> services_and_types; @@ -134,6 +136,48 @@ NodeGraph::get_service_names_and_types() const return services_and_types; } +std::map> +NodeGraph::get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) 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(); + rcl_ret_t ret = rcl_get_service_names_and_types_by_node( + node_base_->get_rcl_node_handle(), + &allocator, + node_name.c_str(), + namespace_.c_str(), + &service_names_and_types); + if (ret != RCL_RET_OK) { + auto error_msg = std::string("failed to get service names and types by node: ") + + rcl_get_error_string().str; + rcl_reset_error(); + if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) { + error_msg += + std::string(", failed also to cleanup service names and types, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + throw std::runtime_error(error_msg); + } + + std::map> services_and_types; + for (size_t i = 0; i < service_names_and_types.names.size; ++i) { + std::string service_name = service_names_and_types.names.data[i]; + for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) { + services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]); + } + } + + ret = rcl_names_and_types_fini(&service_names_and_types); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "could not destroy service names and types"); + } + + return services_and_types; +} + std::vector NodeGraph::get_node_names() const { diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 7ebf833fb1..5eed721191 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -483,6 +483,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, std::map> get_service_names_and_types() const; + /// Return a map of existing service names to list of service types for a specific node. + /** + * This function only considers services - not clients. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + */ + RCLCPP_LIFECYCLE_PUBLIC + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const; + /// Return the number of publishers that are advertised on a given topic. /** * \sa rclcpp::Node::count_publishers diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 7b36f1f753..bf784195a4 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -279,6 +279,15 @@ LifecycleNode::get_service_names_and_types() const return node_graph_->get_service_names_and_types(); } +std::map> +LifecycleNode::get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const +{ + return node_graph_->get_service_names_and_types_by_node( + node_name, namespace_); +} + size_t LifecycleNode::count_publishers(const std::string & topic_name) const { From 5077e1b8cfc9d023ecc36140e8d597872ac01706 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Tue, 26 May 2020 11:47:53 -0700 Subject: [PATCH 063/121] add add_on and remove_on Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- .../rclcpp_lifecycle/lifecycle_node.hpp | 20 +++++++++++++++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 11 ++++++++++ 2 files changed, 31 insertions(+) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 5eed721191..416bf5b53f 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -447,9 +447,29 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const; + using OnSetParametersCallbackHandle = + rclcpp::node_interfaces::OnSetParametersCallbackHandle; using OnParametersSetCallbackType = rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; + /// Add a callback for when parameters are being set. + /** + * \sa rclcpp::Node::add_on_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle::SharedPtr + add_on_set_parameters_callback( + rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType callback); + + /// Remove a callback registered with `add_on_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_on_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + void + remove_on_set_parameters_callback( + const rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle * const handler); + /// Register a callback to be called anytime a parameter is about to be changed. /** * \sa rclcpp::Node::set_on_parameters_set_callback diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index bf784195a4..cdba353a21 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -255,6 +255,17 @@ LifecycleNode::list_parameters( return node_parameters_->list_parameters(prefixes, depth); } +rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr +LifecycleNode::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +{ + return node_parameters_->add_on_set_parameters_callback(callback); +} + +void +LifecycleNode::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback) +{ + return node_parameters_->remove_on_set_parameters_callback(callback); + rclcpp::Node::OnParametersSetCallbackType LifecycleNode::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) { From 76518eb5f6d1fe62927095249568bedd9216351a Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Tue, 26 May 2020 12:17:41 -0700 Subject: [PATCH 064/121] add } Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- rclcpp_lifecycle/src/lifecycle_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index cdba353a21..09e5ed0f0d 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -265,6 +265,7 @@ void LifecycleNode::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback) { return node_parameters_->remove_on_set_parameters_callback(callback); +} rclcpp::Node::OnParametersSetCallbackType LifecycleNode::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) From 7619d2cd4cb5019ed7b0c90f9a96d39ee3aa3e84 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Tue, 26 May 2020 13:28:25 -0700 Subject: [PATCH 065/121] remove return from void fn Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- rclcpp/src/rclcpp/node.cpp | 2 +- rclcpp_lifecycle/src/lifecycle_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index b1f9e66f6a..082370598a 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -331,7 +331,7 @@ Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * co rclcpp::Node::OnParametersSetCallbackType Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) { - return node_parameters_->set_on_parameters_set_callback(callback); + node_parameters_->set_on_parameters_set_callback(callback); } std::vector diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 09e5ed0f0d..a65874054b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -264,7 +264,7 @@ LifecycleNode::add_on_set_parameters_callback(OnParametersSetCallbackType callba void LifecycleNode::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback) { - return node_parameters_->remove_on_set_parameters_callback(callback); + node_parameters_->remove_on_set_parameters_callback(callback); } rclcpp::Node::OnParametersSetCallbackType From 13607a5b120aaf17ad81571279f2dd476260fcb2 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Tue, 26 May 2020 14:36:40 -0700 Subject: [PATCH 066/121] revert Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- rclcpp/src/rclcpp/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 082370598a..b1f9e66f6a 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -331,7 +331,7 @@ Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * co rclcpp::Node::OnParametersSetCallbackType Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) { - node_parameters_->set_on_parameters_set_callback(callback); + return node_parameters_->set_on_parameters_set_callback(callback); } std::vector From 33d926f490f59f0e5a38b91f9d397ec826bdb313 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Tue, 26 May 2020 14:39:11 -0700 Subject: [PATCH 067/121] reduce line length Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- rclcpp_lifecycle/src/lifecycle_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index a65874054b..2ca04f0e0b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -262,7 +262,8 @@ LifecycleNode::add_on_set_parameters_callback(OnParametersSetCallbackType callba } void -LifecycleNode::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback) +LifecycleNode::remove_on_set_parameters_callback( + const OnSetParametersCallbackHandle * const callback) { node_parameters_->remove_on_set_parameters_callback(callback); } From 4b3ef4cc310fdd7a6ab9da50b9693c484fb9dafc Mon Sep 17 00:00:00 2001 From: Claire Wang <22240514+claireyywang@users.noreply.github.com> Date: Tue, 26 May 2020 19:19:20 -0700 Subject: [PATCH 068/121] Deprecate set_on_parameters_set_callback (#1123) * add deprecate statement * replace tests to use add_on_param fn * deprecate set_on_pram fn in node_parameters * deprecate in lifecycle_node and add replacement fn * update documentation * add warning suppression to test_node.cpp * correct namespace in lifecycle_node.cpp * remove whitespace fix line length in lifecycle_node * move reset fn to below add_on * deprecate set_on in test_lifecycle_node * suppress deprecation warning in node.cpp * suppress warning in lifecycle_node.cpp Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- rclcpp/include/rclcpp/node.hpp | 16 ++++--- .../node_interfaces/node_parameters.hpp | 1 + .../node_parameters_interface.hpp | 2 + rclcpp/src/rclcpp/node.cpp | 16 +++++++ rclcpp/test/test_node.cpp | 44 +++++++++++++------ .../rclcpp_lifecycle/lifecycle_node.hpp | 2 + rclcpp_lifecycle/src/lifecycle_node.cpp | 16 +++++++ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 2 +- 8 files changed, 77 insertions(+), 22 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index b73b62d720..2f3c00f4f7 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -277,7 +277,7 @@ class Node : public std::enable_shared_from_this * If `ignore_override` is `true`, the parameter override will be ignored. * * This method, if successful, will result in any callback registered with - * set_on_parameters_set_callback to be called. + * add_on_set_parameters_callback to be called. * If that callback prevents the initial value for the parameter from being * set then rclcpp::exceptions::InvalidParameterValueException is thrown. * @@ -359,7 +359,7 @@ class Node : public std::enable_shared_from_this * by the function call will be ignored. * * This method, if successful, will result in any callback registered with - * set_on_parameters_set_callback to be called, once for each parameter. + * add_on_set_parameters_callback to be called, once for each parameter. * If that callback prevents the initial value for any parameter from being * set then rclcpp::exceptions::InvalidParameterValueException is thrown. * @@ -401,7 +401,7 @@ class Node : public std::enable_shared_from_this /// Undeclare a previously declared parameter. /** * This method will not cause a callback registered with - * set_on_parameters_set_callback to be called. + * add_on_set_parameters_callback to be called. * * \param[in] name The name of the parameter to be undeclared. * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter @@ -435,7 +435,7 @@ class Node : public std::enable_shared_from_this * Parameter overrides are ignored by set_parameter. * * This method will result in any callback registered with - * set_on_parameters_set_callback to be called. + * add_on_set_parameters_callback to be called. * If the callback prevents the parameter from being set, then it will be * reflected in the SetParametersResult that is returned, but no exception * will be thrown. @@ -476,7 +476,7 @@ class Node : public std::enable_shared_from_this * corresponding SetParametersResult in the vector returned by this function. * * This method will result in any callback registered with - * set_on_parameters_set_callback to be called, once for each parameter. + * add_on_set_parameters_callback to be called, once for each parameter. * If the callback prevents the parameter from being set, then, as mentioned * before, it will be reflected in the corresponding SetParametersResult * that is returned, but no exception will be thrown. @@ -507,7 +507,7 @@ class Node : public std::enable_shared_from_this * If the exception is thrown then none of the parameters will have been set. * * This method will result in any callback registered with - * set_on_parameters_set_callback to be called, just one time. + * add_on_set_parameters_callback to be called, just one time. * If the callback prevents the parameters from being set, then it will be * reflected in the SetParametersResult which is returned, but no exception * will be thrown. @@ -787,7 +787,7 @@ class Node : public std::enable_shared_from_this * of the {get,list,describe}_parameter() methods), but may *not* modify * other parameters (by calling any of the {set,declare}_parameter() methods) * or modify the registered callback itself (by calling the - * set_on_parameters_set_callback() method). If a callback tries to do any + * add_on_set_parameters_callback() method). If a callback tries to do any * of the latter things, * rclcpp::exceptions::ParameterModifiedInCallbackException will be thrown. * @@ -839,6 +839,7 @@ class Node : public std::enable_shared_from_this /// Register a callback to be called anytime a parameter is about to be changed. /** + * \deprecated Use add_on_set_parameters_callback instead. * With this method, only one callback can be set at a time. The callback that was previously * set by this method is returned or `nullptr` if no callback was previously set. * @@ -851,6 +852,7 @@ class Node : public std::enable_shared_from_this * \return The previous callback that was registered, if there was one, * otherwise nullptr. */ + [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]] RCLCPP_PUBLIC OnParametersSetCallbackType set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback); diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 4227904af7..cf2676810a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -167,6 +167,7 @@ class NodeParameters : public NodeParametersInterface void remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override; + [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]] RCLCPP_PUBLIC OnParametersSetCallbackType set_on_parameters_set_callback(OnParametersSetCallbackType callback) override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index f1102c0afd..c44763a09a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -193,8 +193,10 @@ class NodeParametersInterface /// Register a callback for when parameters are being set, return an existing one. /** + * \deprecated Use add_on_set_parameters_callback instead. * \sa rclcpp::Node::set_on_parameters_set_callback */ + [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]] RCLCPP_PUBLIC virtual OnParametersSetCallbackType diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index b1f9e66f6a..778d33ed01 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -328,12 +328,28 @@ Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * co return node_parameters_->remove_on_set_parameters_callback(callback); } +// suppress deprecated function warning +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + rclcpp::Node::OnParametersSetCallbackType Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) { return node_parameters_->set_on_parameters_set_callback(callback); } +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + std::vector Node::get_node_names() const { diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp index 3d034651ae..f5cd2a2ae9 100644 --- a/rclcpp/test/test_node.cpp +++ b/rclcpp/test/test_node.cpp @@ -349,7 +349,6 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) { } { // parameter rejected throws - 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) { @@ -366,7 +365,8 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) { } return result; }; - EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset EXPECT_THROW( {node->declare_parameter(name, "not an int");}, rclcpp::exceptions::InvalidParameterValueException); @@ -546,7 +546,6 @@ TEST_F(TestNode, declare_parameter_with_overrides) { } { // parameter rejected throws, with initial value - 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) { @@ -565,7 +564,8 @@ TEST_F(TestNode, declare_parameter_with_overrides) { } return result; }; - EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset EXPECT_THROW( {node->declare_parameter(name, 43);}, rclcpp::exceptions::InvalidParameterValueException); @@ -659,7 +659,6 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { } { // parameter rejected throws - 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) { @@ -676,7 +675,8 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { } return result; }; - EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset EXPECT_THROW( {node->declare_parameters("", {{name, "not an int"}});}, rclcpp::exceptions::InvalidParameterValueException); @@ -854,7 +854,6 @@ 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; @@ -862,7 +861,8 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { result.reason = "no parameter may not be set right now"; return result; }; - node->set_on_parameters_set_callback(on_set_parameters); + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); } @@ -1350,7 +1350,6 @@ 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; @@ -1361,7 +1360,8 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { } return result; }; - node->set_on_parameters_set_callback(on_set_parameters); + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset auto rets = node->set_parameters( { @@ -1527,7 +1527,6 @@ 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; @@ -1538,7 +1537,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { } return result; }; - node->set_on_parameters_set_callback(on_set_parameters); + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset auto ret = node->set_parameters_atomically( { @@ -1638,7 +1638,6 @@ 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; @@ -1649,7 +1648,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { } return result; }; - node->set_on_parameters_set_callback(on_set_parameters); + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset auto ret = node->set_parameters_atomically( { @@ -2329,6 +2329,15 @@ TEST_F(TestNode, get_parameter_types_undeclared_parameters_allowed) { } } +// suppress deprecated function test warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + // test that it is possible to call get_parameter within the set_callback TEST_F(TestNode, set_on_parameters_set_callback_get_parameter) { auto node = std::make_shared("test_set_callback_get_parameter_node"_unq); @@ -2513,6 +2522,13 @@ TEST_F(TestNode, set_on_parameters_set_callback_set_on_parameters_set_callback) }, rclcpp::exceptions::ParameterModifiedInCallbackException); } +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + void expect_qos_profile_eq( const rmw_qos_profile_t & qos1, const rmw_qos_profile_t & qos2, bool is_publisher) { diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 416bf5b53f..5c234e8f2d 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -472,8 +472,10 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, /// Register a callback to be called anytime a parameter is about to be changed. /** + * \deprecated Use add_on_set_parameters_callback instead. * \sa rclcpp::Node::set_on_parameters_set_callback */ + [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]] RCLCPP_LIFECYCLE_PUBLIC rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType set_on_parameters_set_callback( diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 2ca04f0e0b..1d7210546c 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -268,12 +268,28 @@ LifecycleNode::remove_on_set_parameters_callback( node_parameters_->remove_on_set_parameters_callback(callback); } +// suppress deprecated function warning +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + rclcpp::Node::OnParametersSetCallbackType LifecycleNode::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) { return node_parameters_->set_on_parameters_set_callback(callback); } +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + std::vector LifecycleNode::get_node_names() const { diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 57d8b63bf0..713baca8d2 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -427,7 +427,7 @@ TEST_F(TestDefaultStateMachine, check_parameters) { return result; }; - test_node->set_on_parameters_set_callback(callback); + test_node->add_on_set_parameters_callback(callback); rclcpp::Parameter bool_parameter(bool_name, rclcpp::ParameterValue(false)); EXPECT_TRUE(test_node->set_parameter(bool_parameter).successful); EXPECT_EQ(parameters_set, 1u); From 8ea6b9048cef84c863ad8e1d959d0c9b81c8819b Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 26 May 2020 20:40:26 -0700 Subject: [PATCH 069/121] 1.1.0 --- rclcpp/CHANGELOG.rst | 14 ++++++++++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 9 +++++++++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 8 ++++++++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 10 ++++++++++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 45 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 96f3b49d67..6feea5a194 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-05-26) +------------------ +* Deprecate set_on_parameters_set_callback (`#1123 `_) +* Expose get_service_names_and_types_by_node from rcl in rclcpp (`#1131 `_) +* Fix thread safety issues related to logging (`#1125 `_) +* Make sure rmw_publisher_options is initialized in to_rcl_publisher_options (`#1099 `_) +* Remove empty lines within method signatures (`#1128 `_) +* Add API review March 2020 document (`#1031 `_) +* Improve documentation (`#1106 `_) +* Make test multi threaded executor more reliable (`#1105 `_) +* Fixed rep links and added more details to dependencies in quality declaration (`#1116 `_) +* Update quality declarations to reflect version 1.0 (`#1115 `_) +* Contributors: Alejandro Hernández Cordero, ChenYing Kuo, Claire Wang, Dirk Thomas, Ivan Santiago Paunovic, William Woodall, Stephen Brawner + 1.0.0 (2020-05-12) ------------------ * Remove MANUAL_BY_NODE liveliness API (`#1107 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 5303db96e6..7572b72505 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 1.0.0 + 1.1.0 The ROS client library in C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 20871ce281..54db58e063 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,15 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-05-26) +------------------ +* Action client holds weak pointers to goal handles (`#1122 `_) +* Deprecate ClientGoalHandle::async_result() (`#1120 `_) +* Improve documentation (`#1106 `_) +* Fixed rep links and added more details to dependencies in quality declaration (`#1116 `_) +* Update quality declaration to reflect version 1.0 (`#1115 `_) +* Contributors: Alejandro Hernández Cordero, Jacob Perron, Stephen Brawner + 1.0.0 (2020-05-12) ------------------ diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 3f9c9c2c47..ab314611d2 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 1.0.0 + 1.1.0 Adds action APIs for C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index d8de325198..56b9feee6b 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-05-26) +------------------ +* Improve documentation (`#1106 `_) +* Fixed rep links and added more details to dependencies in quality declaration (`#1116 `_) +* Added dockblock to ComponentManager class (`#1102 `_) +* Update quality declaration to reflect version 1.0 (`#1115 `_) +* Contributors: Alejandro Hernández Cordero, Stephen Brawner + 1.0.0 (2020-05-12) ------------------ * Increasing test coverage of rclcpp_components (`#1044 `_) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index d7e87847a2..eb05cf92e2 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 1.0.0 + 1.1.0 Package containing tools for dynamically loadable components Michael Carroll Apache License 2.0 diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 3e743be5f2..cc948a44f4 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,16 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2020-05-26) +------------------ +* Deprecate set_on_parameters_set_callback (`#1123 `_) +* Add missing parameter callback functions to lifecycle node (`#1134 `_) +* Expose get_service_names_and_types_by_node from rcl in rclcpp (`#1131 `_) +* Improve documentation (`#1106 `_) +* Fixed rep links and added more details to dependencies in quality declaration (`#1116 `_) +* Update quality declaration to reflect version 1.0 (`#1115 `_) +* Contributors: Alejandro Hernández Cordero, Claire Wang, Dirk Thomas, Stephen Brawner + 1.0.0 (2020-05-12) ------------------ * Avoid callback_group deprecation (`#1108 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 811de170d4..853ebb2f99 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 1.0.0 + 1.1.0 Package containing a prototype for lifecycle implementation Karsten Knese Apache License 2.0 From 88fa1798377ddf3fdec1c8bee04e848f09b63c2e Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 27 May 2020 10:21:07 -0300 Subject: [PATCH 070/121] Fix potential Construction/Destruction order problem between global contexts vector and Context of static lifetime (#1132) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/context.hpp | 6 ++ rclcpp/src/rclcpp/context.cpp | 111 +++++++++++++++++++++--------- 2 files changed, 86 insertions(+), 31 deletions(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 61eb27ac88..0bfd61955e 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -44,6 +44,9 @@ class ContextAlreadyInitialized : public std::runtime_error : std::runtime_error("context is already initialized") {} }; +/// Forward declare WeakContextsWrapper +class WeakContextsWrapper; + /// Context which encapsulates shared state between nodes and other similar entities. /** * A context also represents the lifecycle between init and shutdown of rclcpp. @@ -358,6 +361,9 @@ class Context : public std::enable_shared_from_this std::mutex interrupt_guard_cond_handles_mutex_; /// Guard conditions for interrupting of associated wait sets on interrupt_all_wait_sets(). std::unordered_map interrupt_guard_cond_handles_; + + /// Keep shared ownership of global vector of weak contexts + std::shared_ptr weak_contexts_; }; /// Return a copy of the list of context shared pointers. diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index ea1c3ec95a..d938268d9b 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -35,12 +35,83 @@ #include "./logging_mutex.hpp" -/// Mutex to protect initialized contexts. -static std::mutex g_contexts_mutex; -/// Weak list of context to be shutdown by the signal handler. -static std::vector> g_contexts; +using rclcpp::Context; + +namespace rclcpp +{ +/// Class to manage vector of weak pointers to all created contexts +class WeakContextsWrapper +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(WeakContextsWrapper) + + void + add_context(const Context::SharedPtr & context) + { + std::lock_guard guard(mutex_); + weak_contexts_.push_back(context); + } + + void + remove_context(const Context * context) + { + std::lock_guard guard(mutex_); + weak_contexts_.erase( + std::remove_if( + weak_contexts_.begin(), + weak_contexts_.end(), + [context](const Context::WeakPtr weak_context) { + auto locked_context = weak_context.lock(); + if (!locked_context) { + // take advantage and removed expired contexts + return true; + } + return locked_context.get() == context; + } + ), + weak_contexts_.end()); + } + + std::vector + get_contexts() + { + std::lock_guard lock(mutex_); + std::vector shared_contexts; + for (auto it = weak_contexts_.begin(); it != weak_contexts_.end(); /* noop */) { + auto context_ptr = it->lock(); + if (!context_ptr) { + // remove invalid weak context pointers + it = weak_contexts_.erase(it); + } else { + ++it; + shared_contexts.push_back(context_ptr); + } + } + return shared_contexts; + } + +private: + std::vector> weak_contexts_; + std::mutex mutex_; +}; +} // namespace rclcpp + +using rclcpp::WeakContextsWrapper; + +/// Global vector of weak pointers to all contexts +static +WeakContextsWrapper::SharedPtr +get_weak_contexts() +{ + static WeakContextsWrapper::SharedPtr weak_contexts = WeakContextsWrapper::make_shared(); + if (!weak_contexts) { + throw std::runtime_error("weak contexts vector is not valid"); + } + return weak_contexts; +} /// Count of contexts that wanted to initialize the logging system. +static size_t & get_logging_reference_count() { @@ -48,8 +119,6 @@ get_logging_reference_count() return ref_count; } -using rclcpp::Context; - extern "C" { static @@ -168,8 +237,8 @@ Context::init( init_options_ = init_options; - std::lock_guard lock(g_contexts_mutex); - g_contexts.push_back(this->shared_from_this()); + weak_contexts_ = get_weak_contexts(); + weak_contexts_->add_context(this->shared_from_this()); } catch (const std::exception & e) { ret = rcl_shutdown(rcl_context_.get()); rcl_context_.reset(); @@ -238,16 +307,7 @@ Context::shutdown(const std::string & reason) this->interrupt_all_sleep_for(); 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(); ) { - auto shared_context = it->lock(); - if (shared_context.get() == this) { - it = g_contexts.erase(it); - break; - } else { - ++it; - } - } + weak_contexts_->remove_context(this); // shutdown logger if (logging_mutex_) { // logging was initialized by this context @@ -396,17 +456,6 @@ Context::clean_up() std::vector rclcpp::get_contexts() { - std::lock_guard lock(g_contexts_mutex); - std::vector shared_contexts; - for (auto it = g_contexts.begin(); it != g_contexts.end(); /* noop */) { - auto context_ptr = it->lock(); - if (!context_ptr) { - // remove invalid weak context pointers - it = g_contexts.erase(it); - } else { - ++it; - shared_contexts.push_back(context_ptr); - } - } - return shared_contexts; + WeakContextsWrapper::SharedPtr weak_contexts = get_weak_contexts(); + return weak_contexts->get_contexts(); } From e72a6a700c789abaec23e7e9d77b4c59f0a9b5dc Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 27 May 2020 16:59:35 -0300 Subject: [PATCH 071/121] Fix test_lifecycle_node.cpp:check_parameters (#1136) Signed-off-by: Ivan Santiago Paunovic --- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 713baca8d2..dca1f5001b 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -427,7 +427,8 @@ TEST_F(TestDefaultStateMachine, check_parameters) { return result; }; - test_node->add_on_set_parameters_callback(callback); + // Hold callback handle. Callback is valid during the lifetime of this object. + auto callback_handle = test_node->add_on_set_parameters_callback(callback); rclcpp::Parameter bool_parameter(bool_name, rclcpp::ParameterValue(false)); EXPECT_TRUE(test_node->set_parameter(bool_parameter).successful); EXPECT_EQ(parameters_set, 1u); From 8abba2c83d17878de489d2fc54725065e07a3d63 Mon Sep 17 00:00:00 2001 From: tomoya Date: Thu, 28 May 2020 07:06:50 +0900 Subject: [PATCH 072/121] Fix lock-order-inversion (potential deadlock) (#1135) Signed-off-by: Tomoya.Fujita --- rclcpp/src/rclcpp/node_interfaces/node_graph.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 0aa361e38d..4d0d46e348 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -368,9 +368,11 @@ rclcpp::Event::SharedPtr NodeGraph::get_graph_event() { auto event = rclcpp::Event::make_shared(); - std::lock_guard graph_changed_lock(graph_mutex_); - graph_events_.push_back(event); - graph_users_count_++; + { + std::lock_guard graph_changed_lock(graph_mutex_); + graph_events_.push_back(event); + graph_users_count_++; + } // on first call, add node to graph_listener_ if (should_add_to_graph_listener_.exchange(false)) { graph_listener_->add_node(this); From 9730ca7cf8295c07be1e048f557da28244a8b32b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 28 May 2020 08:34:29 +0200 Subject: [PATCH 073/121] Improved rclcpp docblock (#1127) * Improved rclcpp docblock Signed-off-by: ahcorde * Improved docblock Signed-off-by: ahcorde * Included feedback Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde --- rclcpp/include/rclcpp/client.hpp | 25 +++++++ rclcpp/include/rclcpp/context.hpp | 6 ++ rclcpp/include/rclcpp/event.hpp | 16 +++++ rclcpp/include/rclcpp/executor.hpp | 19 ++++- .../executors/multi_threaded_executor.hpp | 4 ++ .../executors/single_threaded_executor.hpp | 1 + .../static_executor_entities_collector.hpp | 72 ++++++++++++++++++- .../static_single_threaded_executor.hpp | 17 ++++- .../rclcpp/expand_topic_or_service_name.hpp | 2 + rclcpp/include/rclcpp/graph_listener.hpp | 2 + rclcpp/include/rclcpp/init_options.hpp | 16 ++++- rclcpp/include/rclcpp/loaned_message.hpp | 10 +-- rclcpp/include/rclcpp/memory_strategies.hpp | 4 ++ rclcpp/include/rclcpp/message_info.hpp | 3 + rclcpp/include/rclcpp/node.hpp | 42 +++++++++++ rclcpp/include/rclcpp/node_options.hpp | 3 + rclcpp/include/rclcpp/parameter_client.hpp | 40 +++++++++++ rclcpp/include/rclcpp/service.hpp | 31 ++++++++ rclcpp/include/rclcpp/subscription.hpp | 10 ++- rclcpp/include/rclcpp/subscription_base.hpp | 11 ++- .../include/rclcpp/subscription_factory.hpp | 6 ++ rclcpp/include/rclcpp/time.hpp | 39 ++++++++++ rclcpp/include/rclcpp/time_source.hpp | 1 + rclcpp/include/rclcpp/timer.hpp | 10 ++- .../subscription_topic_statistics.hpp | 1 + rclcpp/include/rclcpp/utilities.hpp | 9 +-- rclcpp/include/rclcpp/wait_result.hpp | 9 +++ rclcpp/include/rclcpp/wait_set_template.hpp | 3 +- 28 files changed, 393 insertions(+), 19 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index e6b02f9969..41b49245bb 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -119,6 +119,7 @@ class ClientBase /// Wait for a service to be ready. /** * \param timeout maximum time to wait + * \return `true` if the service is ready and the timeout is not over, `false` otherwise */ template bool @@ -194,6 +195,17 @@ class Client : public ClientBase RCLCPP_SMART_PTR_DEFINITIONS(Client) + /// Default constructor. + /** + * The constructor for a Client is almost never called directly. + * Instead, clients should be instantiated through the function + * rclcpp::create_client(). + * + * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup. + * \param[in] node_graph The node graph interface of the corresponding node. + * \param[in] service_name Name of the topic to publish to. + * \param[in] client_options options for the subscription. + */ Client( rclcpp::node_interfaces::NodeBaseInterface * node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, @@ -248,12 +260,20 @@ class Client : public ClientBase return this->take_type_erased_response(&response_out, request_header_out); } + /// Create a shared pointer with the response type + /** + * \return shared pointer with the response type + */ std::shared_ptr create_response() override { return std::shared_ptr(new typename ServiceT::Response()); } + /// Create a shared pointer with a rmw_request_id_t + /** + * \return shared pointer with a rmw_request_id_t + */ std::shared_ptr create_request_header() override { @@ -262,6 +282,11 @@ class Client : public ClientBase return std::shared_ptr(new rmw_request_id_t); } + /// Handle a server response + /** + * \param[in] request_header used to check if the secuence number is valid + * \param[in] response message with the server response + */ void handle_response( std::shared_ptr request_header, diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 0bfd61955e..57e7375392 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -103,6 +103,9 @@ class Context : public std::enable_shared_from_this * \param[in] argv argument array which may contain arguments intended for ROS * \param[in] init_options initialization options for rclcpp and underlying layers * \throw ContextAlreadyInitialized if called if init is called more than once + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + * \throws std::runtime_error if the global logging configure mutex is NULL + * \throws exceptions::UnknownROSArgsError if there are unknown ROS arguments */ RCLCPP_PUBLIC virtual @@ -263,6 +266,7 @@ class Context : public std::enable_shared_from_this * \param[in] wait_set Pointer to the rcl_wait_set_t that will be using the * resulting guard condition. * \return Pointer to the guard condition. + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. */ RCLCPP_PUBLIC rcl_guard_condition_t * @@ -282,6 +286,8 @@ class Context : public std::enable_shared_from_this * * \param[in] wait_set Pointer to the rcl_wait_set_t that was using the * resulting guard condition. + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + * \throws std::runtime_error if a nonexistent wait set is trying to release sigint guard condition. */ RCLCPP_PUBLIC void diff --git a/rclcpp/include/rclcpp/event.hpp b/rclcpp/include/rclcpp/event.hpp index 988dba29e2..715eb40ad5 100644 --- a/rclcpp/include/rclcpp/event.hpp +++ b/rclcpp/include/rclcpp/event.hpp @@ -29,17 +29,33 @@ class Event public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Event) + /// Default construct + /** + * Set the default value to false + */ RCLCPP_PUBLIC Event(); + /// Set the Event state value to true + /** + * \return The state value before the call. + */ RCLCPP_PUBLIC bool set(); + /// Get the state value of the Event + /** + * \return the Event state value + */ RCLCPP_PUBLIC bool check(); + /// Get the state value of the Event and set to false + /** + * \return the Event state value + */ RCLCPP_PUBLIC bool check_and_clear(); diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 4f71ae0052..39acba3ec4 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -88,6 +88,9 @@ class Executor add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); /// Convenience function which takes Node and forwards NodeBaseInterface. + /** + * \see rclcpp::Executor::add_node + */ RCLCPP_PUBLIC virtual void add_node(std::shared_ptr node_ptr, bool notify = true); @@ -104,6 +107,9 @@ class Executor remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); /// Convenience function which takes Node and forwards NodeBaseInterface. + /** + * \see rclcpp::Executor::remove_node + */ RCLCPP_PUBLIC virtual void remove_node(std::shared_ptr node_ptr, bool notify = true); @@ -232,7 +238,10 @@ class Executor } /// Cancel any running spin* function, causing it to return. - /* This function can be called asynchonously from any thread. */ + /** + * This function can be called asynchonously from any thread. + * \throws std::runtime_error if there is an issue triggering the guard condition + */ RCLCPP_PUBLIC void cancel(); @@ -242,6 +251,7 @@ class Executor * Switching the memory strategy while the executor is spinning in another threading could have * unintended consequences. * \param[in] memory_strategy Shared pointer to the memory strategy to set. + * \throws std::runtime_error if memory_strategy is null */ RCLCPP_PUBLIC void @@ -255,8 +265,10 @@ class Executor 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, + /** + * \param[in] any_exec Union structure that can hold any executable type (timer, subscription, * service, client). + * \throws std::runtime_error if there is an issue triggering the guard condition */ RCLCPP_PUBLIC void @@ -279,6 +291,9 @@ class Executor static void execute_client(rclcpp::ClientBase::SharedPtr client); + /** + * \throws std::runtime_error if the wait set can be cleared + */ RCLCPP_PUBLIC void wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 7cc82c88db..062b6fbbb7 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -60,6 +60,10 @@ class MultiThreadedExecutor : public rclcpp::Executor RCLCPP_PUBLIC virtual ~MultiThreadedExecutor(); + /** + * \sa rclcpp::Executor:spin() for more details + * \throws std::runtime_error when spin() called while already spinning + */ RCLCPP_PUBLIC void spin() override; diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp index be1310f8df..9dc6dec57b 100644 --- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -59,6 +59,7 @@ class SingleThreadedExecutor : public rclcpp::Executor * the process until canceled. * It may be interrupt by a call to rclcpp::Executor::cancel() or by ctrl-c * if the associated context is configured to shutdown on SIGINT. + * \throws std::runtime_error when spin() called while already spinning */ RCLCPP_PUBLIC void diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index f9a0525112..f4c8e3544c 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp @@ -47,6 +47,13 @@ class StaticExecutorEntitiesCollector final // Destructor ~StaticExecutorEntitiesCollector(); + /// Initialize StaticExecutorEntitiesCollector + /** + * \param p_wait_set A reference to the wait set to be used in the executor + * \param memory_strategy Shared pointer to the memory strategy to set. + * \param executor_guard_condition executor's guard condition + * \throws std::runtime_error if memory strategy is null + */ RCLCPP_PUBLIC void init( @@ -67,16 +74,26 @@ class StaticExecutorEntitiesCollector final fill_executable_list(); /// Function to reallocate space for entities in the wait set. + /** + * \throws std::runtime_error if wait set couldn't be cleared or resized. + */ RCLCPP_PUBLIC void prepare_wait_set(); /// Function to add_handles_to_wait_set and wait for work and - // block until the wait set is ready or until the timeout has been exceeded. + /** + * block until the wait set is ready or until the timeout has been exceeded. + * \throws std::runtime_error if wait set couldn't be cleared or filled. + * \throws any rcl errors from rcl_wait, \sa rclcpp::exceptions::throw_from_rcl_error() + */ RCLCPP_PUBLIC void refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + /** + * \throws std::runtime_error if it couldn't add guard condition to wait set + */ RCLCPP_PUBLIC bool add_to_wait_set(rcl_wait_set_t * wait_set) override; @@ -85,11 +102,19 @@ class StaticExecutorEntitiesCollector final size_t get_number_of_ready_guard_conditions() override; + /** + * \sa rclcpp::Executor::add_node() + * \throw std::runtime_error if node was already added + */ RCLCPP_PUBLIC void add_node( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + /** + * \sa rclcpp::Executor::remove_node() + * \throw std::runtime_error if no guard condition is associated with node. + */ RCLCPP_PUBLIC bool remove_node( @@ -105,42 +130,87 @@ class StaticExecutorEntitiesCollector final bool is_ready(rcl_wait_set_t * wait_set) override; + /// Return number of timers + /** + * \return number of timers + */ RCLCPP_PUBLIC size_t get_number_of_timers() {return exec_list_.number_of_timers;} + /// Return number of subscriptions + /** + * \return number of subscriptions + */ RCLCPP_PUBLIC size_t get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;} + /// Return number of services + /** + * \return number of services + */ RCLCPP_PUBLIC size_t get_number_of_services() {return exec_list_.number_of_services;} + /// Return number of clients + /** + * \return number of clients + */ RCLCPP_PUBLIC size_t get_number_of_clients() {return exec_list_.number_of_clients;} + /// Return number of waitables + /** + * \return number of waitables + */ RCLCPP_PUBLIC size_t get_number_of_waitables() {return exec_list_.number_of_waitables;} + /** Return a SubscritionBase Sharedptr by index. + * \param[in] i The index of the SubscritionBase + * \return a SubscritionBase shared pointer + * \throws std::out_of_range if the argument is higher than the size of the structrue. + */ RCLCPP_PUBLIC rclcpp::SubscriptionBase::SharedPtr get_subscription(size_t i) {return exec_list_.subscription[i];} + /** Return a TimerBase Sharedptr by index. + * \param[in] i The index of the TimerBase + * \return a TimerBase shared pointer + * \throws std::out_of_range if the argument is higher than the size. + */ RCLCPP_PUBLIC rclcpp::TimerBase::SharedPtr get_timer(size_t i) {return exec_list_.timer[i];} + /** Return a ServiceBase Sharedptr by index. + * \param[in] i The index of the ServiceBase + * \return a ServiceBase shared pointer + * \throws std::out_of_range if the argument is higher than the size. + */ RCLCPP_PUBLIC rclcpp::ServiceBase::SharedPtr get_service(size_t i) {return exec_list_.service[i];} + /** Return a ClientBase Sharedptr by index + * \param[in] i The index of the ClientBase + * \return a ClientBase shared pointer + * \throws std::out_of_range if the argument is higher than the size. + */ RCLCPP_PUBLIC rclcpp::ClientBase::SharedPtr get_client(size_t i) {return exec_list_.client[i];} + /** Return a Waitable Sharedptr by index + * \param[in] i The index of the Waitable + * \return a Waitable shared pointer + * \throws std::out_of_range if the argument is higher than the size. + */ RCLCPP_PUBLIC rclcpp::Waitable::SharedPtr get_waitable(size_t i) {return exec_list_.waitable[i];} diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index bf68985812..1e1668bcb3 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -69,8 +69,11 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor virtual ~StaticSingleThreadedExecutor(); /// Static executor implementation of spin. - // 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). + /** + * This function will block until work comes in, execute it, and keep blocking. + * It will only be interrupted by a CTRL-C (managed by the global signal handler). + * \throws std::runtime_error when spin() called while already spinning + */ RCLCPP_PUBLIC void spin() override; @@ -82,6 +85,8 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor * \param[in] notify True to trigger the interrupt guard condition during this function. If * the executor is blocked at the rmw layer while waiting for work and it is notified that a new * node was added, it will wake up. + * \throw std::runtime_error if node was already added or if rcl_trigger_guard_condition + * return an error */ RCLCPP_PUBLIC void @@ -90,6 +95,10 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor bool notify = true) override; /// Convenience function which takes Node and forwards NodeBaseInterface. + /** + * \throw std::runtime_error if node was already added or if rcl_trigger_guard_condition + * returns an error + */ RCLCPP_PUBLIC void add_node(std::shared_ptr node_ptr, bool notify = true) override; @@ -100,6 +109,7 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor * \param[in] notify True to trigger the interrupt guard condition and wake up the executor. * This is useful if the last node was removed from the executor while the executor was blocked * waiting for work in another thread, because otherwise the executor would never be notified. + * \throw std::runtime_error if rcl_trigger_guard_condition returns an error */ RCLCPP_PUBLIC void @@ -108,6 +118,9 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor bool notify = true) override; /// Convenience function which takes Node and forwards NodeBaseInterface. + /** + * \throw std::runtime_error if rcl_trigger_guard_condition returns an error + */ RCLCPP_PUBLIC void remove_node(std::shared_ptr node_ptr, bool notify = true) override; diff --git a/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp b/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp index 5454dfac87..79fbc3e104 100644 --- a/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp +++ b/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp @@ -49,6 +49,8 @@ namespace rclcpp * \throws InvalidServiceNameError if name is invalid and is_service is true * \throws std::bad_alloc if memory cannot be allocated * \throws RCLError if an unexpect error occurs + * \throws std::runtime_error if the topic name is unexpectedly valid or, + * if the rcl name is invalid or if the rcl namespace is invalid */ RCLCPP_PUBLIC std::string diff --git a/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp/include/rclcpp/graph_listener.hpp index 2adbad4e42..a9871701cb 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -73,6 +73,8 @@ class GraphListener : public std::enable_shared_from_this * This function is thread-safe. * * \throws GraphListenerShutdownError if the GraphListener is shutdown + * \throws std::runtime if the parent context was destroyed + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. */ RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/init_options.hpp b/rclcpp/include/rclcpp/init_options.hpp index 7b05ee8d00..447c148e00 100644 --- a/rclcpp/include/rclcpp/init_options.hpp +++ b/rclcpp/include/rclcpp/init_options.hpp @@ -30,11 +30,21 @@ class InitOptions /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed). bool shutdown_on_sigint = true; - /// Constructor which allows you to specify the allocator used within the init options. + /// Constructor + /** + * It allows you to specify the allocator used within the init options. + * \param[in] allocator used allocate memory within the init options + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + */ RCLCPP_PUBLIC explicit InitOptions(rcl_allocator_t allocator = rcl_get_default_allocator()); /// Constructor which is initialized by an existing init_options. + /** + * Initialized by an existing init_options. + * \param[in] init_options rcl_init_options_t to initialized + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + */ RCLCPP_PUBLIC explicit InitOptions(const rcl_init_options_t & init_options); @@ -62,6 +72,10 @@ class InitOptions ~InitOptions(); /// Return the rcl init options. + /** + * \return the rcl init options. + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + */ RCLCPP_PUBLIC const rcl_init_options_t * get_rcl_init_options() const; diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index ad086cfd64..50a8766fda 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -52,8 +52,9 @@ class LoanedMessage * However, this user code is ought to be usable even when dynamically linked against * a middleware which doesn't support message loaning in which case the allocator will be used. * - * \param pub rclcpp::Publisher instance to which the memory belongs - * \param allocator Allocator instance in case middleware can not allocate messages + * \param[in] pub rclcpp::Publisher instance to which the memory belongs + * \param[in] allocator Allocator instance in case middleware can not allocate messages + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. */ LoanedMessage( const rclcpp::PublisherBase & pub, @@ -98,8 +99,9 @@ class LoanedMessage * However, this user code is ought to be usable even when dynamically linked against * a middleware which doesn't support message loaning in which case the allocator will be used. * - * \param pub rclcpp::Publisher instance to which the memory belongs - * \param allocator Allocator instance in case middleware can not allocate messages + * \param[in] pub rclcpp::Publisher instance to which the memory belongs + * \param[in] allocator Allocator instance in case middleware can not allocate messages + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. */ LoanedMessage( const rclcpp::PublisherBase * pub, diff --git a/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp index a3950d9bd0..12730fc6f6 100644 --- a/rclcpp/include/rclcpp/memory_strategies.hpp +++ b/rclcpp/include/rclcpp/memory_strategies.hpp @@ -23,6 +23,10 @@ namespace rclcpp namespace memory_strategies { +/// Create a MemoryStrategy sharedPtr +/** + * \return a MemoryStrategy sharedPtr + */ RCLCPP_PUBLIC memory_strategy::MemoryStrategy::SharedPtr create_default_strategy(); diff --git a/rclcpp/include/rclcpp/message_info.hpp b/rclcpp/include/rclcpp/message_info.hpp index 1653b0a505..452712afdb 100644 --- a/rclcpp/include/rclcpp/message_info.hpp +++ b/rclcpp/include/rclcpp/message_info.hpp @@ -30,6 +30,9 @@ class RCLCPP_PUBLIC MessageInfo MessageInfo() = default; /// Conversion constructor, which is intentionally not marked as explicit. + /** + * \param[in] rmw_message_info message info to initialize the class + */ // cppcheck-suppress noExplicitConstructor MessageInfo(const rmw_message_info_t & rmw_message_info); // NOLINT(runtime/explicit) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 2f3c00f4f7..f379367d2e 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -78,6 +78,7 @@ class Node : public std::enable_shared_from_this /** * \param[in] node_name Name of the node. * \param[in] options Additional options to control creation of the node. + * \throws InvalidNamespaceError if the namespace is invalid */ RCLCPP_PUBLIC explicit Node( @@ -89,6 +90,7 @@ class Node : public std::enable_shared_from_this * \param[in] node_name Name of the node. * \param[in] namespace_ Namespace of the node. * \param[in] options Additional options to control creation of the node. + * \throws InvalidNamespaceError if the namespace is invalid */ RCLCPP_PUBLIC explicit Node( @@ -122,6 +124,7 @@ class Node : public std::enable_shared_from_this /// Get the fully-qualified name of the node. /** * The fully-qualified name includes the local namespace and name of the node. + * \return fully-qualified name of the node. */ RCLCPP_PUBLIC const char * @@ -685,6 +688,7 @@ class Node : public std::enable_shared_from_this * \throws rclcpp::exceptions::ParameterNotDeclaredException if the * parameter has not been declared and undeclared parameters are not * allowed. + * \throws std::runtime_error if the number of described parameters is more than one */ RCLCPP_PUBLIC rcl_interfaces::msg::ParameterDescriptor @@ -707,6 +711,7 @@ class Node : public std::enable_shared_from_this * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the * parameters have not been declared and undeclared parameters are not * allowed. + * \throws std::runtime_error if the number of described parameters is more than one */ RCLCPP_PUBLIC std::vector @@ -866,14 +871,30 @@ class Node : public std::enable_shared_from_this std::vector get_node_names() const; + /// Return a map of existing topic names to list of topic types. + /** + * \return a map of existing topic names to list of topic types. + * \throws std::runtime_error anything that rcl_error can throw + */ RCLCPP_PUBLIC std::map> get_topic_names_and_types() const; + /// Return a map of existing service names to list of service types. + /** + * \return a map of existing service names to list of service types. + * \throws std::runtime_error anything that rcl_error can throw + */ RCLCPP_PUBLIC std::map> get_service_names_and_types() const; + /// Return the number of publishers that are advertised on a given topic. + /** + * \param[in] topic_name the topic_name on which to count the publishers. + * \return number of publishers that are advertised on a given topic. + * \throws std::runtime_error if publishers could not be counted + */ RCLCPP_PUBLIC std::map> get_service_names_and_types_by_node( @@ -884,6 +905,12 @@ class Node : public std::enable_shared_from_this size_t count_publishers(const std::string & topic_name) const; + /// Return the number of subscribers who have created a subscription for a given topic. + /** + * \param[in] topic_name the topic_name on which to count the subscribers. + * \return number of subscribers who have created a subscription for a given topic. + * \throws std::runtime_error if publishers could not be counted + */ RCLCPP_PUBLIC size_t count_subscribers(const std::string & topic_name) const; @@ -953,6 +980,9 @@ class Node : public std::enable_shared_from_this /** * The given Event must be acquire through the get_graph_event() method. * + * \param[in] event pointer to an Event to wait for + * \param[in] timeout nanoseconds to wait for the Event to change the state + * * \throws InvalidEventError if the given event is nullptr * \throws EventNotRegisteredError if the given event was not acquired with * get_graph_event(). @@ -963,14 +993,26 @@ class Node : public std::enable_shared_from_this rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout); + /// Get a clock as a non-const shared pointer which is managed by the node. + /** + * \sa rclcpp::node_interfaces::NodeClock::get_clock + */ RCLCPP_PUBLIC rclcpp::Clock::SharedPtr get_clock(); + /// Get a clock as a const shared pointer which is managed by the node. + /** + * \sa rclcpp::node_interfaces::NodeClock::get_clock + */ RCLCPP_PUBLIC rclcpp::Clock::ConstSharedPtr get_clock() const; + /// Returns current time from the time source specified by clock_type. + /** + * \sa rclcpp::Clock::now + */ RCLCPP_PUBLIC Time now() const; diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 32464c556c..01b0158759 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -77,6 +77,9 @@ class NodeOptions * This data structure is created lazily, on the first call to this function. * Repeated calls will not regenerate it unless one of the input settings * changed, like arguments, use_global_arguments, or the rcl allocator. + * + * \return a const rcl_node_options_t structure used by the node + * \throws exceptions::UnknownROSArgsError if there are unknown ROS arguments */ RCLCPP_PUBLIC const rcl_node_options_t * diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 021ceb37bd..fa45dd285e 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -46,6 +46,16 @@ class AsyncParametersClient public: RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient) + /// Create an async parameters client. + /** + * \param node_base_interface[in] The node base interface of the corresponding node. + * \param node_topics_interface[in] Node topic base interface. + * \param node_graph_interface[in] The node graph interface of the corresponding node. + * \param node_services_interface[in] Node service interface. + * \param remote_node_name[in] (optional) name of the remote node + * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe + * \param group[in] (optional) The async parameter client will be added to this callback group. + */ RCLCPP_PUBLIC AsyncParametersClient( const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, @@ -56,6 +66,13 @@ class AsyncParametersClient const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Constructor + /** + * \param node[in] The async paramters client will be added to this node. + * \param remote_node_name[in] (optional) name of the remote node + * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe + * \param group[in] (optional) The async parameter client will be added to this callback group. + */ RCLCPP_PUBLIC AsyncParametersClient( const rclcpp::Node::SharedPtr node, @@ -63,6 +80,13 @@ class AsyncParametersClient const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Constructor + /** + * \param node[in] The async paramters client will be added to this node. + * \param remote_node_name[in] (optional) name of the remote node + * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe + * \param group[in] (optional) The async parameter client will be added to this callback group. + */ RCLCPP_PUBLIC AsyncParametersClient( rclcpp::Node * node, @@ -159,10 +183,26 @@ class AsyncParametersClient options); } + /// Return if the parameter services are ready. + /** + * This method checks the following services: + * - get parameter + * - get parameter + * - set parameters + * - list parameters + * - describe parameters + * + * \return `true` if the service is ready, `false` otherwise + */ RCLCPP_PUBLIC bool service_is_ready() const; + /// Wait for the services to be ready. + /** + * \param timeout maximum time to wait + * \return `true` if the services are ready and the timeout is not over, `false` otherwise + */ template bool wait_for_service( diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index abe25867f0..2ba6b7f6a3 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -156,6 +156,17 @@ class Service : public ServiceBase std::shared_ptr)>; RCLCPP_SMART_PTR_DEFINITIONS(Service) + /// Default constructor. + /** + * The constructor for a Service is almost never called directly. + * Instead, services should be instantiated through the function + * rclcpp::create_service(). + * + * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. + * \param[in] service_name Name of the topic to publish to. + * \param[in] any_callback User defined callback to call when a client request is received. + * \param[in] service_options options for the subscription. + */ Service( std::shared_ptr node_handle, const std::string & service_name, @@ -219,6 +230,16 @@ class Service : public ServiceBase #endif } + /// Default constructor. + /** + * The constructor for a Service is almost never called directly. + * Instead, services should be instantiated through the function + * rclcpp::create_service(). + * + * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. + * \param[in] service_name Name of the topic to publish to. + * \param[in] any_callback User defined callback to call when a client request is received. + */ Service( std::shared_ptr node_handle, std::shared_ptr service_handle, @@ -244,6 +265,16 @@ class Service : public ServiceBase #endif } + /// Default constructor. + /** + * The constructor for a Service is almost never called directly. + * Instead, services should be instantiated through the function + * rclcpp::create_service(). + * + * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. + * \param[in] service_name Name of the topic to publish to. + * \param[in] any_callback User defined callback to call when a client request is received. + */ Service( std::shared_ptr node_handle, rcl_service_t * service_handle, diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 97e8714b40..b567d1910e 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -94,7 +94,7 @@ class Subscription : public SubscriptionBase * \param[in] callback User defined callback to call when a message is received. * \param[in] options options for the subscription. * \param[in] message_memory_strategy The memory strategy to be used for managing message memory. - * \param[in] subscription_topic_statistics pointer to a topic statistics subcription. + * \param[in] subscription_topic_statistics Optional pointer to a topic statistics subcription. * \throws std::invalid_argument if the QoS is uncompatible with intra-process (if one * of the following conditions are true: qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL, * qos_profile.depth == 0 or qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE). @@ -295,7 +295,9 @@ class Subscription : public SubscriptionBase } /// Return the borrowed message. - /** \param message message to be returned */ + /** + * \param[inout] message message to be returned + */ void return_message(std::shared_ptr & message) override { @@ -303,6 +305,10 @@ class Subscription : public SubscriptionBase message_memory_strategy_->return_message(typed_message); } + /// Return the borrowed serialized message. + /** + * \param[inout] message serialized message to be returned + */ void return_serialized_message(std::shared_ptr & message) override { diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 8156622299..84bb127e60 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -110,6 +110,7 @@ class SubscriptionBase : public std::enable_shared_from_this * May throw runtime_error when an unexpected error occurs. * * \return The actual qos settings. + * \throws std::runtime_error if failed to get qos settings */ RCLCPP_PUBLIC rclcpp::QoS @@ -201,6 +202,10 @@ class SubscriptionBase : public std::enable_shared_from_this const rosidl_message_type_support_t & get_message_type_support_handle() const; + /// Return if the subscription is serialized + /** + * \return `true` if the subscription is serialized, `false` otherwise + */ RCLCPP_PUBLIC bool is_serialized() const; @@ -232,7 +237,11 @@ class SubscriptionBase : public std::enable_shared_from_this uint64_t intra_process_subscription_id, IntraProcessManagerWeakPtr weak_ipm); - /// Return the waitable for intra-process, or nullptr if intra-process is not setup. + /// Return the waitable for intra-process + /** + * \return the waitable sharedpointer for intra-process, or nullptr if intra-process is not setup. + * \throws std::runtime_error if the intra process manager is destroyed + */ RCLCPP_PUBLIC rclcpp::Waitable::SharedPtr get_intra_process_waitable() const; diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index ffab0724c8..fc3933ebcf 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -64,6 +64,12 @@ struct SubscriptionFactory }; /// Return a SubscriptionFactory setup to create a SubscriptionT. +/** + * \param[in] callback The user-defined callback function to receive a message + * \param[in] options Additional options for the creation of the Subscription. + * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. + * \param[in] subscription_topic_Optinal stats callback for topic_statistics + */ template< typename MessageT, typename CallbackT, diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index a80201a356..64c6126316 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -40,6 +40,7 @@ class Time * \param seconds part of the time in seconds since time epoch * \param nanoseconds part of the time in nanoseconds since time epoch * \param clock_type clock type + * \throws std::runtime_error if seconds are negative */ RCLCPP_PUBLIC Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); @@ -60,6 +61,7 @@ class Time /** * \param time_msg builtin_interfaces time message to copy * \param clock_type clock type + * \throws std::runtime_error if seconds are negative */ RCLCPP_PUBLIC Time( @@ -82,6 +84,9 @@ class Time RCLCPP_PUBLIC operator builtin_interfaces::msg::Time() const; + /** + * \throws std::runtime_error if seconds are negative + */ RCLCPP_PUBLIC Time & operator=(const Time & rhs); @@ -90,6 +95,9 @@ class Time Time & operator=(const builtin_interfaces::msg::Time & time_msg); + /** + * \throws std::runtime_error if the time sources are different + */ RCLCPP_PUBLIC bool operator==(const rclcpp::Time & rhs) const; @@ -98,38 +106,66 @@ class Time bool operator!=(const rclcpp::Time & rhs) const; + /** + * \throws std::runtime_error if the time sources are different + */ RCLCPP_PUBLIC bool operator<(const rclcpp::Time & rhs) const; + /** + * \throws std::runtime_error if the time sources are different + */ RCLCPP_PUBLIC bool operator<=(const rclcpp::Time & rhs) const; + /** + * \throws std::runtime_error if the time sources are different + */ RCLCPP_PUBLIC bool operator>=(const rclcpp::Time & rhs) const; + /** + * \throws std::runtime_error if the time sources are different + */ RCLCPP_PUBLIC bool operator>(const rclcpp::Time & rhs) const; + /** + * \throws std::overflow_error if addition leads to overflow + */ RCLCPP_PUBLIC Time operator+(const rclcpp::Duration & rhs) const; + /** + * \throws std::runtime_error if the time sources are different + * \throws std::overflow_error if addition leads to overflow + */ RCLCPP_PUBLIC Duration operator-(const rclcpp::Time & rhs) const; + /** + * \throws std::overflow_error if addition leads to overflow + */ RCLCPP_PUBLIC Time operator-(const rclcpp::Duration & rhs) const; + /** + * \throws std::overflow_error if addition leads to overflow + */ RCLCPP_PUBLIC Time & operator+=(const rclcpp::Duration & rhs); + /** + * \throws std::overflow_error if addition leads to overflow + */ RCLCPP_PUBLIC Time & operator-=(const rclcpp::Duration & rhs); @@ -174,6 +210,9 @@ class Time friend Clock; // Allow clock to manipulate internal data }; +/** + * \throws std::overflow_error if addition leads to overflow + */ Time operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs); diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 86f62a6cab..74bf38e894 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -87,6 +87,7 @@ class TimeSource /// Attach a clock to the time source to be updated /** + * \param[in] clock to attach to the time source * \throws std::invalid_argument the time source must be a RCL_ROS_TIME otherwise throws an exception */ RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 3f3b4d80b7..7fc375dace 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -100,7 +100,10 @@ class TimerBase 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. */ + /** + * \return A std::chrono::duration representing the relative time until the next callback. + * \throws std::runtime_error if the rcl_timer_get_time_until_next_call returns a failure + */ RCLCPP_PUBLIC std::chrono::nanoseconds time_until_trigger(); @@ -114,6 +117,7 @@ class TimerBase * This function expects its caller to immediately trigger the callback after this function, * since it maintains the last time the callback was triggered. * \return True if the timer needs to trigger. + * \throws std::runtime_error if it failed to check timer */ RCLCPP_PUBLIC bool is_ready(); @@ -185,6 +189,10 @@ class GenericTimer : public TimerBase cancel(); } + /** + * \sa rclcpp::TimerBase::execute_callback + * \throws std::runtime_error if it failed to notify timer that callback occurred + */ void execute_callback() override { diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index 37a2804cbb..e450aa5d65 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -75,6 +75,7 @@ class SubscriptionTopicStatistics * topic source * \param publisher instance constructed by the node in order to publish statistics data. * This class owns the publisher. + * \throws std::invalid_argument if publisher pointer is nullptr */ SubscriptionTopicStatistics( const std::string & node_name, diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index d876b2d1d9..2b6a3db37f 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -136,7 +136,7 @@ remove_ros_arguments(int argc, char const * const argv[]); * If nullptr is given for the context, then the global context is used, i.e. * the context initialized by rclcpp::init(). * - * \param[in] context Check for shutdown of this Context. + * \param[in] context Optional check for shutdown of this Context. * \return true if shutdown has been called, false otherwise */ RCLCPP_PUBLIC @@ -150,7 +150,7 @@ ok(rclcpp::Context::SharedPtr context = nullptr); * * Deprecated, as it is no longer different from rcl_ok(). * - * \param[in] context Check for initialization of this Context. + * \param[in] context Optional check for initialization of this Context. * \return true if the context is initialized, and false otherwise */ [[deprecated("use the function ok() instead, which has the same usage.")]] @@ -168,7 +168,8 @@ is_initialized(rclcpp::Context::SharedPtr context = nullptr); * This will also cause the "on_shutdown" callbacks to be called. * * \sa rclcpp::Context::shutdown() - * \param[in] context to be shutdown + * \param[in] context Optional to be shutdown + * \param[in] reason Optional string passed to the context shutdown method * \return true if shutdown was successful, false if context was already shutdown */ RCLCPP_PUBLIC @@ -206,7 +207,7 @@ on_shutdown(std::function callback, rclcpp::Context::SharedPtr context = * the context initialized by rclcpp::init(). * * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for. - * \param[in] context which may interrupt this sleep + * \param[in] context Optional which may interrupt this sleep * \return true if the condition variable did not timeout. */ RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index 0dfa6974ea..e879043d04 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -58,6 +58,7 @@ class WaitResult final /** * \param[in] wait_set A reference to the wait set, which this class * will keep for the duration of its lifetime. + * \return a WaitResult from a "ready" result. */ static WaitResult @@ -90,6 +91,10 @@ class WaitResult final } /// Return the rcl wait set. + /** + * \return const rcl wait set. + * \throws std::runtime_error if the class cannot access wait set when the result was not ready + */ const WaitSetT & get_wait_set() const { @@ -102,6 +107,10 @@ class WaitResult final } /// Return the rcl wait set. + /** + * \return rcl wait set. + * \throws std::runtime_error if the class cannot access wait set when the result was not ready + */ WaitSetT & get_wait_set() { diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 1c9b072275..2ab6f57ba1 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -643,7 +643,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli * when time_to_wait is < 0, or * \returns Empty if the wait set is empty, avoiding the possibility of * waiting indefinitely on an empty wait set. - * \throws rclcpp::exceptions::RCLError on unhandled rcl errors + * \throws rclcpp::exceptions::RCLError on unhandled rcl errors or, + * \throws std::runtime_error if unknown WaitResultKind */ template RCUTILS_WARN_UNUSED From dbd7c2c84380fd041004779a23f50567df88a7e8 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 20 May 2020 19:54:18 +0000 Subject: [PATCH 074/121] Make the rcl publisher handle a shared pointer. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/loaned_message.hpp | 4 +- rclcpp/include/rclcpp/publisher.hpp | 14 +++--- rclcpp/include/rclcpp/publisher_base.hpp | 8 ++-- rclcpp/src/rclcpp/publisher_base.cpp | 55 ++++++++++++++---------- 4 files changed, 45 insertions(+), 36 deletions(-) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 50a8766fda..3b94d86ae0 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -66,7 +66,7 @@ class LoanedMessage if (pub_.can_loan_messages()) { void * message_ptr = nullptr; auto ret = rcl_borrow_loaned_message( - pub_.get_publisher_handle(), + pub_.get_publisher_handle().get(), rosidl_typesupport_cpp::get_message_type_support_handle(), &message_ptr); if (RCL_RET_OK != ret) { @@ -139,7 +139,7 @@ class LoanedMessage if (pub_.can_loan_messages()) { // return allocated memory to the middleware auto ret = - rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle(), message_); + rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle().get(), message_); if (ret != RCL_RET_OK) { RCLCPP_ERROR( error_logger, "rcl_deallocate_loaned_message failed: %s", rcl_get_error_string().str); diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index ebe349cd99..8518353574 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -279,12 +279,12 @@ class Publisher : public PublisherBase void do_inter_process_publish(const MessageT & msg) { - auto status = rcl_publish(&publisher_handle_, &msg, nullptr); + auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { 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 (rcl_publisher_is_valid_except_context(publisher_handle_.get())) { + rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get()); if (nullptr != context && !rcl_context_is_valid(context)) { // publisher is invalid due to context being shutdown return; @@ -303,7 +303,7 @@ class Publisher : public PublisherBase // TODO(Karsten1987): support serialized message passed by intraprocess throw std::runtime_error("storing serialized messages in intra process is not supported yet"); } - auto status = rcl_publish_serialized_message(&publisher_handle_, serialized_msg, nullptr); + auto status = rcl_publish_serialized_message(publisher_handle_.get(), serialized_msg, nullptr); if (RCL_RET_OK != status) { rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message"); } @@ -312,12 +312,12 @@ class Publisher : public PublisherBase void do_loaned_message_publish(MessageT * msg) { - auto status = rcl_publish_loaned_message(&publisher_handle_, msg, nullptr); + auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg, nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { 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 (rcl_publisher_is_valid_except_context(publisher_handle_.get())) { + rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get()); if (nullptr != context && !rcl_context_is_valid(context)) { // publisher is invalid due to context being shutdown return; diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 9627acaa4a..0afe29b3f4 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -99,13 +99,13 @@ class PublisherBase : public std::enable_shared_from_this /// Get the rcl publisher handle. /** \return The rcl publisher handle. */ RCLCPP_PUBLIC - rcl_publisher_t * + std::shared_ptr get_publisher_handle(); /// Get the rcl publisher handle. /** \return The rcl publisher handle. */ RCLCPP_PUBLIC - const rcl_publisher_t * + std::shared_ptr get_publisher_handle() const; /// Get all the QoS event handlers associated with this publisher. @@ -203,7 +203,7 @@ class PublisherBase : public std::enable_shared_from_this auto handler = std::make_shared>( callback, rcl_publisher_event_init, - &publisher_handle_, + publisher_handle_.get(), event_type); event_handlers_.emplace_back(handler); } @@ -213,7 +213,7 @@ class PublisherBase : public std::enable_shared_from_this std::shared_ptr rcl_node_handle_; - rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); + std::shared_ptr publisher_handle_; std::vector> event_handlers_; diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index a10f2e17f7..decdf0b81f 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -47,8 +47,24 @@ PublisherBase::PublisherBase( : rcl_node_handle_(node_base->get_shared_rcl_node_handle()), intra_process_is_enabled_(false), intra_process_publisher_id_(0) { + auto custom_deleter = [node_handle = this->rcl_node_handle_](rcl_publisher_t * rcl_pub) + { + if (rcl_publisher_fini(rcl_pub, node_handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle.get()).get_child("rclcpp"), + "Error in destruction of rcl publisher handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete rcl_pub; + }; + + publisher_handle_ = std::shared_ptr( + new rcl_publisher_t, custom_deleter); + *publisher_handle_.get() = rcl_get_zero_initialized_publisher(); + rcl_ret_t ret = rcl_publisher_init( - &publisher_handle_, + publisher_handle_.get(), rcl_node_handle_.get(), &type_support, topic.c_str(), @@ -67,7 +83,7 @@ PublisherBase::PublisherBase( rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher"); } // Life time of this object is tied to the publisher handle. - rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_); + rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(publisher_handle_.get()); if (!publisher_rmw_handle) { auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string().str; rcl_reset_error(); @@ -85,14 +101,6 @@ PublisherBase::~PublisherBase() // must fini the events before fini-ing the publisher event_handlers_.clear(); - 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); - rcl_reset_error(); - } - auto ipm = weak_ipm_.lock(); if (!intra_process_is_enabled_) { @@ -102,7 +110,7 @@ PublisherBase::~PublisherBase() // TODO(ivanpauno): should this raise an error? RCLCPP_WARN( rclcpp::get_logger("rclcpp"), - "Intra process manager died before than a publisher."); + "Intra process manager died before a publisher."); return; } ipm->remove_publisher(intra_process_publisher_id_); @@ -111,13 +119,14 @@ PublisherBase::~PublisherBase() const char * PublisherBase::get_topic_name() const { - return rcl_publisher_get_topic_name(&publisher_handle_); + return rcl_publisher_get_topic_name(publisher_handle_.get()); } size_t PublisherBase::get_queue_size() const { - const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_); + const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options( + publisher_handle_.get()); if (!publisher_options) { auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string().str; rcl_reset_error(); @@ -132,16 +141,16 @@ PublisherBase::get_gid() const return rmw_gid_; } -rcl_publisher_t * +std::shared_ptr PublisherBase::get_publisher_handle() { - return &publisher_handle_; + return publisher_handle_; } -const rcl_publisher_t * +std::shared_ptr PublisherBase::get_publisher_handle() const { - return &publisher_handle_; + return publisher_handle_; } const std::vector> & @@ -156,13 +165,13 @@ PublisherBase::get_subscription_count() const size_t inter_process_subscription_count = 0; rcl_ret_t status = rcl_publisher_get_subscription_count( - &publisher_handle_, + publisher_handle_.get(), &inter_process_subscription_count); if (RCL_RET_PUBLISHER_INVALID == status) { 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 (rcl_publisher_is_valid_except_context(publisher_handle_.get())) { + rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get()); if (nullptr != context && !rcl_context_is_valid(context)) { /* publisher is invalid due to context being shutdown */ return 0; @@ -195,7 +204,7 @@ PublisherBase::get_intra_process_subscription_count() const rclcpp::QoS PublisherBase::get_actual_qos() const { - const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(&publisher_handle_); + const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(publisher_handle_.get()); if (!qos) { auto msg = std::string("failed to get qos settings: ") + rcl_get_error_string().str; rcl_reset_error(); @@ -208,13 +217,13 @@ PublisherBase::get_actual_qos() const bool PublisherBase::assert_liveliness() const { - return RCL_RET_OK == rcl_publisher_assert_liveliness(&publisher_handle_); + return RCL_RET_OK == rcl_publisher_assert_liveliness(publisher_handle_.get()); } bool PublisherBase::can_loan_messages() const { - return rcl_publisher_can_loan_messages(&publisher_handle_); + return rcl_publisher_can_loan_messages(publisher_handle_.get()); } bool From 7a6db002c6a80a968c1f537ff601235cbd674d6b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 May 2020 20:48:26 +0000 Subject: [PATCH 075/121] Hold onto the rcl_{subscription,publisher}_t shared_ptr. This keeps it from going out of scope while the executor is still dealing with QoSEvents. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/publisher_base.hpp | 5 +++-- rclcpp/include/rclcpp/qos_event.hpp | 8 +++++--- rclcpp/include/rclcpp/subscription_base.hpp | 5 +++-- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 0afe29b3f4..48ebfea860 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -200,10 +200,11 @@ class PublisherBase : public std::enable_shared_from_this const EventCallbackT & callback, const rcl_publisher_event_type_t event_type) { - auto handler = std::make_shared>( + auto handler = std::make_shared>>( callback, rcl_publisher_event_init, - publisher_handle_.get(), + publisher_handle_, event_type); event_handlers_.emplace_back(handler); } diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index ad011f8595..a43d904f10 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -102,11 +102,11 @@ class QOSEventHandlerBase : public Waitable size_t wait_set_event_index_; }; -template +template class QOSEventHandler : public QOSEventHandlerBase { public: - template + template QOSEventHandler( const EventCallbackT & callback, InitFuncT init_func, @@ -114,8 +114,9 @@ class QOSEventHandler : public QOSEventHandlerBase EventTypeEnum event_type) : event_callback_(callback) { + parent_handle_ = parent_handle; event_handle_ = rcl_get_zero_initialized_event(); - rcl_ret_t ret = init_func(&event_handle_, parent_handle, event_type); + rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type); if (ret != RCL_RET_OK) { if (ret == RCL_RET_UNSUPPORTED) { UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event"); @@ -148,6 +149,7 @@ class QOSEventHandler : public QOSEventHandlerBase using EventCallbackInfoT = typename std::remove_reference::template argument_type<0>>::type; + ParentHandleT parent_handle_; EventCallbackT event_callback_; }; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 84bb127e60..cb560d6a75 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -270,10 +270,11 @@ class SubscriptionBase : public std::enable_shared_from_this const EventCallbackT & callback, const rcl_subscription_event_type_t event_type) { - auto handler = std::make_shared>( + auto handler = std::make_shared>>( callback, rcl_subscription_event_init, - get_subscription_handle().get(), + get_subscription_handle(), event_type); qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false)); event_handlers_.emplace_back(handler); From 580892b8a6c1d5843dde4223eea9ebab36d4ebd4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 May 2020 21:56:11 +0000 Subject: [PATCH 076/121] Make sure the Waitable class has a virtual destructor. Noticed while reviewing this issue. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/waitable.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 2f282349ae..428cc1d660 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -30,6 +30,9 @@ class Waitable public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Waitable) + RCLCPP_PUBLIC + virtual ~Waitable() = default; + /// Get the number of ready subscriptions /** * Returns a value of 0 by default. From e7763e9cd104e6aad0653356bdddfdec38560770 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 28 May 2020 16:48:20 -0300 Subject: [PATCH 077/121] `SubscriptionBase::get_subscription_handle() const` should return a shared pointer to const value (#1140) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/subscription_base.hpp | 2 +- rclcpp/src/rclcpp/subscription_base.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index cb560d6a75..68df9211bd 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -91,7 +91,7 @@ class SubscriptionBase : public std::enable_shared_from_this get_subscription_handle(); RCLCPP_PUBLIC - const std::shared_ptr + std::shared_ptr get_subscription_handle() const; /// Get all the QoS event handlers associated with this subscription. diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index dc8c559ffa..37726031b4 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -109,7 +109,7 @@ SubscriptionBase::get_subscription_handle() return subscription_handle_; } -const std::shared_ptr +std::shared_ptr SubscriptionBase::get_subscription_handle() const { return subscription_handle_; From 488732f2434e625815f6c57f16c87135d45ed0b0 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 28 May 2020 18:48:53 -0300 Subject: [PATCH 078/121] Pass shared pointer by value instead than by const & when possible (#1141) Signed-off-by: Ivan Santiago Paunovic --- .../rclcpp/wait_set_policies/dynamic_storage.hpp | 12 ++++++------ .../rclcpp/wait_set_policies/static_storage.hpp | 15 ++++++++------- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 907fc86bdf..f930ab4c4f 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -52,9 +52,9 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo /// Conversion constructor, which is intentionally not marked explicit. SubscriptionEntry( - const std::shared_ptr & subscription_in = nullptr, + std::shared_ptr subscription_in = nullptr, const rclcpp::SubscriptionWaitSetMask & mask_in = {}) - : subscription(subscription_in), + : subscription(std::move(subscription_in)), mask(mask_in) {} @@ -117,10 +117,10 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo /// Conversion constructor, which is intentionally not marked explicit. WaitableEntry( - const std::shared_ptr & waitable_in = nullptr, - const std::shared_ptr & associated_entity_in = nullptr) noexcept - : waitable(waitable_in), - associated_entity(associated_entity_in) + std::shared_ptr waitable_in = nullptr, + std::shared_ptr associated_entity_in = nullptr) noexcept + : waitable(std::move(waitable_in)), + associated_entity(std::move(associated_entity_in)) {} void diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index d4d6c0c014..434947c19f 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rclcpp/client.hpp" #include "rclcpp/guard_condition.hpp" @@ -60,9 +61,9 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom /// Conversion constructor, which is intentionally not marked explicit. SubscriptionEntry( - const std::shared_ptr & subscription_in = nullptr, - const rclcpp::SubscriptionWaitSetMask & mask_in = {}) - : subscription(subscription_in), + std::shared_ptr subscription_in = nullptr, + rclcpp::SubscriptionWaitSetMask mask_in = {}) + : subscription(std::move(subscription_in)), mask(mask_in) {} }; @@ -100,10 +101,10 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom { /// Conversion constructor, which is intentionally not marked explicit. WaitableEntry( - const std::shared_ptr & waitable_in = nullptr, - const std::shared_ptr & associated_entity_in = nullptr) noexcept - : waitable(waitable_in), - associated_entity(associated_entity_in) + std::shared_ptr waitable_in = nullptr, + std::shared_ptr associated_entity_in = nullptr) noexcept + : waitable(std::move(waitable_in)), + associated_entity(std::move(associated_entity_in)) {} std::shared_ptr waitable; From 535bac73610b2a1410c69e2edab17ddef163fbf7 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 29 May 2020 13:38:36 -0300 Subject: [PATCH 079/121] Add missing header in logging_mutex.cpp (#1145) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/logging_mutex.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclcpp/src/rclcpp/logging_mutex.cpp b/rclcpp/src/rclcpp/logging_mutex.cpp index 5d189c4918..308a21fe73 100644 --- a/rclcpp/src/rclcpp/logging_mutex.cpp +++ b/rclcpp/src/rclcpp/logging_mutex.cpp @@ -17,6 +17,8 @@ #include "rcutils/macros.h" +#include "./logging_mutex.hpp" + std::shared_ptr get_global_logging_mutex() { From 316faf18b1cae138baa1e79dc8d884be8006d71d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 29 May 2020 16:49:45 -0400 Subject: [PATCH 080/121] Add Security Vulnerability Policy pointing to REP-2006. (#1130) Signed-off-by: Chris Lalancette --- rclcpp/QUALITY_DECLARATION.md | 2 +- rclcpp_action/QUALITY_DECLARATION.md | 2 +- rclcpp_components/QUALITY_DECLARATION.md | 2 +- rclcpp_lifecycle/QUALITY_DECLARATION.md | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/QUALITY_DECLARATION.md b/rclcpp/QUALITY_DECLARATION.md index 176771251b..8b2b970887 100644 --- a/rclcpp/QUALITY_DECLARATION.md +++ b/rclcpp/QUALITY_DECLARATION.md @@ -213,4 +213,4 @@ Currently nightly build status can be seen here: ### Vulnerability Disclosure Policy [7.i] -This package does not yet have a Vulnerability Disclosure Policy +This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). diff --git a/rclcpp_action/QUALITY_DECLARATION.md b/rclcpp_action/QUALITY_DECLARATION.md index fc15b0abf7..97f4cb72ab 100644 --- a/rclcpp_action/QUALITY_DECLARATION.md +++ b/rclcpp_action/QUALITY_DECLARATION.md @@ -181,4 +181,4 @@ Currently nightly build status can be seen here: ### Vulnerability Disclosure Policy [7.i] -This package does not yet have a Vulnerability Disclosure Policy +This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). diff --git a/rclcpp_components/QUALITY_DECLARATION.md b/rclcpp_components/QUALITY_DECLARATION.md index 74e50d113e..93d017edbf 100644 --- a/rclcpp_components/QUALITY_DECLARATION.md +++ b/rclcpp_components/QUALITY_DECLARATION.md @@ -189,4 +189,4 @@ Currently nightly build status can be seen here: ### Vulnerability Disclosure Policy [7.i] -This package does not yet have a Vulnerability Disclosure Policy +This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). diff --git a/rclcpp_lifecycle/QUALITY_DECLARATION.md b/rclcpp_lifecycle/QUALITY_DECLARATION.md index afcc1430b7..4452f39e13 100644 --- a/rclcpp_lifecycle/QUALITY_DECLARATION.md +++ b/rclcpp_lifecycle/QUALITY_DECLARATION.md @@ -187,4 +187,4 @@ Currently nightly build status can be seen here: ### Vulnerability Disclosure Policy [7.i] -This package does not yet have a Vulnerability Disclosure Policy +This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). From 743f643f3108a533c254322a28b07d764b780703 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 1 Jun 2020 15:02:38 -0300 Subject: [PATCH 081/121] Make test_rate more reliable on Windows and improve error output when it fails (#1146) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/test/test_rate.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/rclcpp/test/test_rate.cpp b/rclcpp/test/test_rate.cpp index a4c1c4386a..5833e02b21 100644 --- a/rclcpp/test/test_rate.cpp +++ b/rclcpp/test/test_rate.cpp @@ -22,9 +22,9 @@ Basic tests for the Rate and WallRate classes. */ TEST(TestRate, rate_basics) { - auto period = std::chrono::milliseconds(100); - auto offset = std::chrono::milliseconds(50); - auto epsilon = std::chrono::milliseconds(1); + auto period = std::chrono::milliseconds(1000); + auto offset = std::chrono::milliseconds(500); + auto epsilon = std::chrono::milliseconds(100); double overrun_ratio = 1.5; auto start = std::chrono::system_clock::now(); @@ -33,15 +33,15 @@ TEST(TestRate, rate_basics) { ASSERT_TRUE(r.sleep()); auto one = std::chrono::system_clock::now(); auto delta = one - start; - ASSERT_TRUE(period < delta); - ASSERT_TRUE(period * overrun_ratio > delta); + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); rclcpp::sleep_for(offset); ASSERT_TRUE(r.sleep()); auto two = std::chrono::system_clock::now(); delta = two - start; - ASSERT_TRUE(2 * period < delta); - ASSERT_TRUE(2 * period * overrun_ratio > delta); + EXPECT_LT(2 * period, delta); + EXPECT_GT(2 * period * overrun_ratio, delta); rclcpp::sleep_for(offset); auto two_offset = std::chrono::system_clock::now(); @@ -49,8 +49,8 @@ TEST(TestRate, rate_basics) { ASSERT_TRUE(r.sleep()); auto three = std::chrono::system_clock::now(); delta = three - two_offset; - ASSERT_TRUE(period < delta); - ASSERT_TRUE(period * overrun_ratio > delta); + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); rclcpp::sleep_for(offset + period); auto four = std::chrono::system_clock::now(); @@ -72,15 +72,15 @@ TEST(TestRate, wall_rate_basics) { ASSERT_TRUE(r.sleep()); auto one = std::chrono::steady_clock::now(); auto delta = one - start; - ASSERT_TRUE(period < delta); - ASSERT_TRUE(period * overrun_ratio > delta); + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); rclcpp::sleep_for(offset); ASSERT_TRUE(r.sleep()); auto two = std::chrono::steady_clock::now(); delta = two - start; - ASSERT_TRUE(2 * period < delta + epsilon); - ASSERT_TRUE(2 * period * overrun_ratio > delta); + EXPECT_LT(2 * period, delta + epsilon); + EXPECT_GT(2 * period * overrun_ratio, delta); rclcpp::sleep_for(offset); auto two_offset = std::chrono::steady_clock::now(); @@ -88,13 +88,13 @@ TEST(TestRate, wall_rate_basics) { ASSERT_TRUE(r.sleep()); auto three = std::chrono::steady_clock::now(); delta = three - two_offset; - ASSERT_TRUE(period < delta); - ASSERT_TRUE(period * overrun_ratio > delta); + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); rclcpp::sleep_for(offset + period); auto four = std::chrono::steady_clock::now(); ASSERT_FALSE(r.sleep()); auto five = std::chrono::steady_clock::now(); delta = five - four; - ASSERT_TRUE(epsilon > delta); + EXPECT_GT(epsilon, delta); } From 1bce6fb391dcb0bc3907bcdeeca8f342c9ada444 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 1 Jun 2020 20:04:29 -0300 Subject: [PATCH 082/121] Avoid multiple type topics in tests. (#1150) Signed-off-by: Michel Hidalgo --- .../test_subscription_topic_statistics.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 3a7e90c025..74a79e6f9b 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -46,6 +46,7 @@ namespace constexpr const char kTestPubNodeName[]{"test_pub_stats_node"}; constexpr const char kTestSubNodeName[]{"test_sub_stats_node"}; constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"}; +constexpr const char kTestSubStatsEmptyTopic[]{"/test_sub_stats_empty_topic"}; constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}; constexpr const uint64_t kNoSamples{0}; constexpr const std::chrono::seconds kTestDuration{10}; @@ -210,21 +211,20 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test void SetUp() { rclcpp::init(0 /* argc */, nullptr /* argv */); - empty_subscriber = std::make_shared( - kTestSubNodeName, - kTestSubStatsTopic); } void TearDown() { rclcpp::shutdown(); - empty_subscriber.reset(); } - std::shared_ptr empty_subscriber; }; TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) { + auto empty_subscriber = std::make_shared( + kTestSubNodeName, + kTestSubStatsEmptyTopic); + // Manually create publisher tied to the node auto topic_stats_publisher = empty_subscriber->create_publisher( @@ -251,7 +251,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no // Create an empty publisher auto empty_publisher = std::make_shared( kTestPubNodeName, - kTestSubStatsTopic); + kTestSubStatsEmptyTopic); // empty_subscriber has a topic statistics instance as part of its subscription // this will listen to and generate statistics for the empty message @@ -261,6 +261,10 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no "/statistics", 2); + auto empty_subscriber = std::make_shared( + kTestSubNodeName, + kTestSubStatsEmptyTopic); + rclcpp::executors::SingleThreadedExecutor ex; ex.add_node(empty_publisher); ex.add_node(statistics_listener); From 6c807a6be9c0a026e172d41a6d06039249ad1bdb Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 1 Jun 2020 23:58:48 -0300 Subject: [PATCH 083/121] Add missing virtual destructors (#1149) * Add -Wnon-virtual-dtor -Woverloaded-virtual compiler options Signed-off-by: Ivan Santiago Paunovic * Add missing virtual dtors Signed-off-by: Ivan Santiago Paunovic * please linter Signed-off-by: Ivan Santiago Paunovic --- rclcpp/CMakeLists.txt | 2 +- rclcpp/include/rclcpp/rate.hpp | 1 + rclcpp/include/rclcpp/timer.hpp | 1 + rclcpp_action/CMakeLists.txt | 2 +- rclcpp_components/CMakeLists.txt | 2 +- rclcpp_lifecycle/CMakeLists.txt | 2 +- .../include/rclcpp_lifecycle/lifecycle_publisher.hpp | 1 + .../node_interfaces/lifecycle_node_interface.hpp | 4 ++++ 8 files changed, 11 insertions(+), 4 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 171e0f875d..4562375a15 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -23,7 +23,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) endif() set(${PROJECT_NAME}_SRCS diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 296cce14a1..55d3bbcb85 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -31,6 +31,7 @@ class RateBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase) + virtual ~RateBase() {} virtual bool sleep() = 0; virtual bool is_steady() const = 0; virtual void reset() = 0; diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 7fc375dace..66a7529bce 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -62,6 +62,7 @@ class TimerBase /// TimerBase destructor RCLCPP_PUBLIC + virtual ~TimerBase(); /// Cancel the timer. diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 598f37a50f..08c4d23b2c 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -13,7 +13,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) endif() set(${PROJECT_NAME}_SRCS diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index 759e0f3acb..f438a2b0bc 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) endif() find_package(ament_cmake_ros REQUIRED) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 66df83d10e..953cd0d4ef 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) endif() find_package(ament_cmake_ros REQUIRED) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 2f94dc78c0..fdf9ba17c9 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -36,6 +36,7 @@ namespace rclcpp_lifecycle class LifecyclePublisherInterface { public: + virtual ~LifecyclePublisherInterface() {} virtual void on_activate() = 0; virtual void on_deactivate() = 0; virtual bool is_activated() = 0; 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..9f2459e296 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 @@ -100,6 +100,10 @@ class LifecycleNodeInterface RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn on_error(const State & previous_state); + + RCLCPP_LIFECYCLE_PUBLIC + virtual + ~LifecycleNodeInterface() {} }; } // namespace node_interfaces From 1ddc23a77c459d4761470d5c899f0fd180f3d268 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 1 Jun 2020 21:49:59 -0700 Subject: [PATCH 084/121] changelogs Signed-off-by: William Woodall --- rclcpp/CHANGELOG.rst | 15 +++++++++++++++ rclcpp_action/CHANGELOG.rst | 6 ++++++ rclcpp_components/CHANGELOG.rst | 6 ++++++ rclcpp_lifecycle/CHANGELOG.rst | 7 +++++++ 4 files changed, 34 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 6feea5a194..d7d4172a38 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added missing virtual destructors. (`#1149 `_) +* Fixed a test which was using different types on the same topic. (`#1150 `_) +* Made ``test_rate`` more reliable on Windows and improve error output when it fails (`#1146 `_) +* Added Security Vulnerability Policy pointing to REP-2006. (`#1130 `_) +* Added missing header in ``logging_mutex.cpp``. (`#1145 `_) +* Changed the WaitSet API to pass a shared pointer by value instead than by const reference when possible. (`#1141 `_) +* Changed ``SubscriptionBase::get_subscription_handle() const`` to return a shared pointer to const value. (`#1140 `_) +* Extended the lifetime of ``rcl_publisher_t`` by holding onto the shared pointer in order to avoid a use after free situation. (`#1119 `_) +* Improved some docblocks (`#1127 `_) +* Fixed a lock-order-inversion (potential deadlock) (`#1135 `_) +* Fixed a potential Construction/Destruction order problem between global contexts vector and Context of static lifetime (`#1132 `_) +* Contributors: Alejandro Hernández Cordero, Chris Lalancette, Ivan Santiago Paunovic, Michel Hidalgo, tomoya + 1.1.0 (2020-05-26) ------------------ * Deprecate set_on_parameters_set_callback (`#1123 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 54db58e063..5e29b7f877 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added missing virtual destructors. (`#1149 `_) +* Add Security Vulnerability Policy pointing to REP-2006. (`#1130 `_) +* Contributors: Chris Lalancette, Ivan Santiago Paunovic + 1.1.0 (2020-05-26) ------------------ * Action client holds weak pointers to goal handles (`#1122 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 56b9feee6b..621d47f023 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added missing virtual destructors. (`#1149 `_) +* Add Security Vulnerability Policy pointing to REP-2006. (`#1130 `_) +* Contributors: Chris Lalancette, Ivan Santiago Paunovic + 1.1.0 (2020-05-26) ------------------ * Improve documentation (`#1106 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index cc948a44f4..32856a1c2a 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added missing virtual destructors. (`#1149 `_) +* Add Security Vulnerability Policy pointing to REP-2006. (`#1130 `_) +* Fixed ``test_lifecycle_node.cpp:check_parameters`` (`#1136 `_) +* Contributors: Chris Lalancette, Ivan Santiago Paunovic + 1.1.0 (2020-05-26) ------------------ * Deprecate set_on_parameters_set_callback (`#1123 `_) From 50112e5815ec94c19b845dcb6d9dbf419c66d6ea Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 1 Jun 2020 21:54:47 -0700 Subject: [PATCH 085/121] 2.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index d7d4172a38..6212be7884 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2020-06-01) +------------------ * Added missing virtual destructors. (`#1149 `_) * Fixed a test which was using different types on the same topic. (`#1150 `_) * Made ``test_rate`` more reliable on Windows and improve error output when it fails (`#1146 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 7572b72505..edbcc7fcdc 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 1.1.0 + 2.0.0 The ROS client library in C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 5e29b7f877..2e59cd044f 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2020-06-01) +------------------ * Added missing virtual destructors. (`#1149 `_) * Add Security Vulnerability Policy pointing to REP-2006. (`#1130 `_) * Contributors: Chris Lalancette, Ivan Santiago Paunovic diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index ab314611d2..4dfa1764f2 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 1.1.0 + 2.0.0 Adds action APIs for C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 621d47f023..b096270a3a 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2020-06-01) +------------------ * Added missing virtual destructors. (`#1149 `_) * Add Security Vulnerability Policy pointing to REP-2006. (`#1130 `_) * Contributors: Chris Lalancette, Ivan Santiago Paunovic diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index eb05cf92e2..8d004a7f58 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 1.1.0 + 2.0.0 Package containing tools for dynamically loadable components Michael Carroll Apache License 2.0 diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 32856a1c2a..ea6404470d 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2020-06-01) +------------------ * Added missing virtual destructors. (`#1149 `_) * Add Security Vulnerability Policy pointing to REP-2006. (`#1130 `_) * Fixed ``test_lifecycle_node.cpp:check_parameters`` (`#1136 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 853ebb2f99..10e9a6f657 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 1.1.0 + 2.0.0 Package containing a prototype for lifecycle implementation Karsten Knese Apache License 2.0 From f7bc020a261658574045dbb720cda178cfa8564f Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 8 Jun 2020 11:03:34 -0300 Subject: [PATCH 086/121] Increase rclcpp_action test coverage (#1153) Signed-off-by: Michel Hidalgo --- .../include/rclcpp_action/create_client.hpp | 25 ++- .../include/rclcpp_action/create_server.hpp | 25 ++- rclcpp_action/test/test_client.cpp | 163 +++++++++++++++--- rclcpp_action/test/test_server.cpp | 129 ++++++++++++-- 4 files changed, 277 insertions(+), 65 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 2acd4d7aee..2c4d495ea4 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -59,20 +59,19 @@ create_client( 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 (shared_node) { + // 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 specific group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } } } delete ptr; diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 2886ee9c97..b579fea800 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -78,20 +78,19 @@ create_server( 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 (shared_node) { + // 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 specific group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } } } delete ptr; diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 8735717003..4b631e785d 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -32,6 +32,7 @@ #include +#include #include #include #include @@ -73,7 +74,7 @@ class TestClient : public ::testing::Test rclcpp::init(0, nullptr); } - void SetUp() + void SetUpServer() { rcl_allocator_t allocator = rcl_get_default_allocator(); @@ -211,7 +212,10 @@ class TestClient : public ::testing::Test ASSERT_TRUE(status_publisher != nullptr); allocator.deallocate(status_topic_name, allocator.state); server_executor.add_node(server_node); + } + void SetUp() override + { client_node = std::make_shared(client_node_name, namespace_name); client_executor.add_node(client_node); @@ -219,7 +223,12 @@ class TestClient : public ::testing::Test ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1))); } - void TearDown() + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void TearDownServer() { status_publisher.reset(); feedback_publisher.reset(); @@ -227,6 +236,10 @@ class TestClient : public ::testing::Test result_service.reset(); goal_service.reset(); server_node.reset(); + } + + void TearDown() override + { client_node.reset(); } @@ -265,11 +278,37 @@ class TestClient : public ::testing::Test typename rclcpp::Publisher::SharedPtr status_publisher; }; +class TestClientAgainstServer : public TestClient +{ +protected: + void SetUp() override + { + SetUpServer(); + TestClient::SetUp(); + } + + void TearDown() override + { + TestClient::TearDown(); + TearDownServer(); + } +}; + + TEST_F(TestClient, construction_and_destruction) { ASSERT_NO_THROW(rclcpp_action::create_client(client_node, action_name).reset()); } +TEST_F(TestClient, construction_and_destruction_after_node) +{ + ASSERT_NO_THROW( + { + auto action_client = rclcpp_action::create_client(client_node, action_name); + client_node.reset(); + }); +} + TEST_F(TestClient, construction_and_destruction_callback_group) { auto group = client_node->create_callback_group( @@ -285,7 +324,24 @@ TEST_F(TestClient, construction_and_destruction_callback_group) ).reset()); } -TEST_F(TestClient, async_send_goal_no_callbacks) +TEST_F(TestClient, wait_for_action_server) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + EXPECT_FALSE(action_client->wait_for_action_server(0ms)); + EXPECT_FALSE(action_client->wait_for_action_server(100ms)); + auto future = std::async( + std::launch::async, [&action_client]() { + return action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT); + }); + SetUpServer(); + EXPECT_TRUE(future.get()); + TearDownServer(); + + client_node.reset(); // Drop node before action client + EXPECT_THROW(action_client->wait_for_action_server(0ms), rclcpp::exceptions::InvalidNodeError); +} + +TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -306,7 +362,24 @@ TEST_F(TestClient, async_send_goal_no_callbacks) EXPECT_FALSE(goal_handle->is_result_aware()); } -TEST_F(TestClient, async_send_goal_no_callbacks_wait_for_result) +TEST_F(TestClientAgainstServer, bad_goal_handles) +{ + auto action_client0 = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client0->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 0; + auto future_goal_handle = action_client0->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + + auto action_client1 = rclcpp_action::create_client(client_node, action_name); + using rclcpp_action::exceptions::UnknownGoalHandleError; + EXPECT_THROW(action_client1->async_get_result(goal_handle), UnknownGoalHandleError); + EXPECT_THROW(action_client1->async_cancel_goal(goal_handle), UnknownGoalHandleError); +} + +TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_wait_for_result) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -329,13 +402,33 @@ TEST_F(TestClient, async_send_goal_no_callbacks_wait_for_result) EXPECT_EQ(5, wrapped_result.result->sequence[5]); } -TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result) +TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_then_invalidate) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); ActionGoal goal; - goal.order = 4; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + ASSERT_NE(nullptr, goal_handle); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + + action_client.reset(); // Ensure goal handle is invalidated once client goes out of scope + + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status()); + using rclcpp_action::exceptions::UnawareGoalHandleError; + EXPECT_THROW(future_result.get(), UnawareGoalHandleError); +} + +TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait_for_result) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + bool goal_response_received = false; auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.goal_response_callback = @@ -347,23 +440,37 @@ TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result) goal_response_received = true; } }; - auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); - auto goal_handle = future_goal_handle.get(); - EXPECT_TRUE(goal_response_received); - EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); - EXPECT_FALSE(goal_handle->is_feedback_aware()); - EXPECT_FALSE(goal_handle->is_result_aware()); - auto future_result = action_client->async_get_result(goal_handle); - EXPECT_TRUE(goal_handle->is_result_aware()); - dual_spin_until_future_complete(future_result); - auto wrapped_result = future_result.get(); - ASSERT_EQ(5u, wrapped_result.result->sequence.size()); - EXPECT_EQ(3, wrapped_result.result->sequence.back()); + { + ActionGoal bad_goal; + bad_goal.order = -1; + auto future_goal_handle = action_client->async_send_goal(bad_goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_FALSE(goal_response_received); + EXPECT_EQ(nullptr, goal_handle); + } + + { + ActionGoal goal; + goal.order = 4; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_response_received); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); + } } -TEST_F(TestClient, async_send_goal_with_feedback_callback_wait_for_result) +TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_result) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -397,7 +504,7 @@ TEST_F(TestClient, async_send_goal_with_feedback_callback_wait_for_result) EXPECT_EQ(5, feedback_count); } -TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result) +TEST_F(TestClientAgainstServer, async_send_goal_with_result_callback_wait_for_result) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -432,7 +539,7 @@ TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result) EXPECT_EQ(3, wrapped_result.result->sequence.back()); } -TEST_F(TestClient, async_get_result_with_callback) +TEST_F(TestClientAgainstServer, async_get_result_with_callback) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -467,7 +574,7 @@ TEST_F(TestClient, async_get_result_with_callback) EXPECT_EQ(3, wrapped_result.result->sequence.back()); } -TEST_F(TestClient, async_cancel_one_goal) +TEST_F(TestClientAgainstServer, async_cancel_one_goal) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -485,7 +592,7 @@ TEST_F(TestClient, async_cancel_one_goal) EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); } -TEST_F(TestClient, async_cancel_one_goal_with_callback) +TEST_F(TestClientAgainstServer, async_cancel_one_goal_with_callback) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -519,7 +626,7 @@ TEST_F(TestClient, async_cancel_one_goal_with_callback) EXPECT_TRUE(cancel_response_received); } -TEST_F(TestClient, async_cancel_all_goals) +TEST_F(TestClientAgainstServer, async_cancel_all_goals) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -555,7 +662,7 @@ TEST_F(TestClient, async_cancel_all_goals) EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status()); } -TEST_F(TestClient, async_cancel_all_goals_with_callback) +TEST_F(TestClientAgainstServer, async_cancel_all_goals_with_callback) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -605,7 +712,7 @@ TEST_F(TestClient, async_cancel_all_goals_with_callback) EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status()); } -TEST_F(TestClient, async_cancel_some_goals) +TEST_F(TestClientAgainstServer, async_cancel_some_goals) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -636,7 +743,7 @@ TEST_F(TestClient, async_cancel_some_goals) EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); } -TEST_F(TestClient, async_cancel_some_goals_with_callback) +TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 7c873f5ee1..0ef8aa5ab8 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -82,17 +82,42 @@ TEST_F(TestServer, construction_and_destruction) { auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); - using GoalHandle = rclcpp_action::ServerGoalHandle; - auto as = rclcpp_action::create_server( - node, "fibonacci", - [](const GoalUUID &, std::shared_ptr) { - return rclcpp_action::GoalResponse::REJECT; - }, - [](std::shared_ptr) { - return rclcpp_action::CancelResponse::REJECT; - }, - [](std::shared_ptr) {}); - (void)as; + ASSERT_NO_THROW( + { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + }); +} + +TEST_F(TestServer, construction_and_destruction_after_node) +{ + auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); + + ASSERT_NO_THROW( + { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + + node.reset(); + }); } TEST_F(TestServer, construction_and_destruction_callback_group) @@ -733,6 +758,84 @@ TEST_F(TestServer, get_result) using GoalHandle = rclcpp_action::ServerGoalHandle; + 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; + }; + + const std::chrono::milliseconds result_timeout{50}; + + rcl_action_server_options_t options = rcl_action_server_get_default_options(); + options.result_timeout.nanoseconds = RCL_MS_TO_NS(result_timeout.count()); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted, + options); + (void)as; + + send_goal_request(node, uuid); + + // Send result request + 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"); + } + auto request = std::make_shared(); + request->goal_id.uuid = uuid; + auto future = result_client->async_send_request(request); + + // Send a result + auto result = std::make_shared(); + result->sequence = {5, 8, 13, 21}; + received_handle->succeed(result); + + // Wait for the result request to be received + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + auto response = future.get(); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); + EXPECT_EQ(result->sequence, response->result.sequence); + + // Wait for goal expiration + rclcpp::sleep_for(2 * result_timeout); + + // Allow for expiration to take place + rclcpp::spin_some(node); + + // Send and wait for another result request + future = result_client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + response = future.get(); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_UNKNOWN, response->status); +} + +TEST_F(TestServer, get_result_deferred) +{ + 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; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto handle_cancel = [](std::shared_ptr) { return rclcpp_action::CancelResponse::REJECT; @@ -763,6 +866,10 @@ TEST_F(TestServer, get_result) request->goal_id.uuid = uuid; auto future = result_client->async_send_request(request); + // Process request first + rclcpp::sleep_for(std::chrono::milliseconds(10)); // Give a chance for the request to be served + rclcpp::spin_some(node); + // Send a result auto result = std::make_shared(); result->sequence = {5, 8, 13, 21}; From 0c8aca2bd9cb33d42a42657f9b3a69c64f8ea5cf Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Tue, 9 Jun 2020 01:09:46 +0530 Subject: [PATCH 087/121] Allow spin_until_future_complete to accept std::future (#1113) Signed-off-by: Sarthak Mittal --- rclcpp/include/rclcpp/executor.hpp | 4 +- rclcpp/include/rclcpp/executors.hpp | 12 ++--- .../static_single_threaded_executor.hpp | 4 +- rclcpp/test/test_executor.cpp | 48 +++++++++++++++++++ 4 files changed, 58 insertions(+), 10 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 39acba3ec4..77fb35ca2e 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -188,10 +188,10 @@ class Executor * code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ - template + template FutureReturnCode spin_until_future_complete( - const std::shared_future & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index ec55d1309e..36fb0d63cf 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -66,12 +66,12 @@ using rclcpp::executors::SingleThreadedExecutor; * If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ -template +template rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const std::shared_future & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete @@ -82,13 +82,13 @@ spin_node_until_future_complete( return retcode; } -template rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, std::shared_ptr node_ptr, - const std::shared_future & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::executors::spin_node_until_future_complete( @@ -104,7 +104,7 @@ template & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { rclcpp::executors::SingleThreadedExecutor executor; @@ -116,7 +116,7 @@ template node_ptr, - const std::shared_future & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 1e1668bcb3..25645e23f2 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -143,10 +143,10 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor * exec.add_node(node); * exec.spin_until_future_complete(future); */ - template + template rclcpp::FutureReturnCode spin_until_future_complete( - std::shared_future & future, + FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { std::future_status status = future.wait_for(std::chrono::seconds(0)); diff --git a/rclcpp/test/test_executor.cpp b/rclcpp/test/test_executor.cpp index 95371415fc..cdefec71e1 100644 --- a/rclcpp/test/test_executor.cpp +++ b/rclcpp/test/test_executor.cpp @@ -67,3 +67,51 @@ TEST_F(TestExecutors, addTemporaryNode) { executor.add_node(std::make_shared("temporary_node")); EXPECT_NO_THROW(executor.spin_some()); } + +// Make sure that the spin_until_future_complete works correctly with std::future +TEST_F(TestExecutors, testSpinUntilFutureComplete) { + rclcpp::executors::SingleThreadedExecutor executor; + std::future future; + rclcpp::FutureReturnCode ret; + + // test success + future = std::async( + []() { + return; + }); + ret = executor.spin_until_future_complete(future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + + // test timeout + future = std::async( + []() { + std::this_thread::sleep_for(1s); + return; + }); + ret = executor.spin_until_future_complete(future, 100ms); + EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); +} + +// Make sure that the spin_until_future_complete works correctly with std::shared_future +TEST_F(TestExecutors, testSpinUntilFutureCompleteSharedFuture) { + rclcpp::executors::SingleThreadedExecutor executor; + std::future future; + rclcpp::FutureReturnCode ret; + + // test success + future = std::async( + []() { + return; + }); + ret = executor.spin_until_future_complete(future.share(), 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + + // test timeout + future = std::async( + []() { + std::this_thread::sleep_for(1s); + return; + }); + ret = executor.spin_until_future_complete(future.share(), 100ms); + EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); +} From 037659cc67b2767bb99a6670606d3f53bdf5d946 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Mon, 8 Jun 2020 21:38:52 -0700 Subject: [PATCH 088/121] Revert "Allow spin_until_future_complete to accept std::future (#1113)" (#1159) This reverts commit 898a30e0e25ddb0742436c71f5eb18681280aad3. --- rclcpp/include/rclcpp/executor.hpp | 4 +- rclcpp/include/rclcpp/executors.hpp | 12 ++--- .../static_single_threaded_executor.hpp | 4 +- rclcpp/test/test_executor.cpp | 48 ------------------- 4 files changed, 10 insertions(+), 58 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 77fb35ca2e..39acba3ec4 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -188,10 +188,10 @@ class Executor * code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ - template + template FutureReturnCode spin_until_future_complete( - const FutureT & future, + const std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 36fb0d63cf..ec55d1309e 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -66,12 +66,12 @@ using rclcpp::executors::SingleThreadedExecutor; * If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ -template +template rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const FutureT & future, + const std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete @@ -82,13 +82,13 @@ spin_node_until_future_complete( return retcode; } -template rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, std::shared_ptr node_ptr, - const FutureT & future, + const std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::executors::spin_node_until_future_complete( @@ -104,7 +104,7 @@ template & future, std::chrono::duration timeout = std::chrono::duration(-1)) { rclcpp::executors::SingleThreadedExecutor executor; @@ -116,7 +116,7 @@ template node_ptr, - const FutureT & future, + const std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 25645e23f2..1e1668bcb3 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -143,10 +143,10 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor * exec.add_node(node); * exec.spin_until_future_complete(future); */ - template + template rclcpp::FutureReturnCode spin_until_future_complete( - FutureT & future, + std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) { std::future_status status = future.wait_for(std::chrono::seconds(0)); diff --git a/rclcpp/test/test_executor.cpp b/rclcpp/test/test_executor.cpp index cdefec71e1..95371415fc 100644 --- a/rclcpp/test/test_executor.cpp +++ b/rclcpp/test/test_executor.cpp @@ -67,51 +67,3 @@ TEST_F(TestExecutors, addTemporaryNode) { executor.add_node(std::make_shared("temporary_node")); EXPECT_NO_THROW(executor.spin_some()); } - -// Make sure that the spin_until_future_complete works correctly with std::future -TEST_F(TestExecutors, testSpinUntilFutureComplete) { - rclcpp::executors::SingleThreadedExecutor executor; - std::future future; - rclcpp::FutureReturnCode ret; - - // test success - future = std::async( - []() { - return; - }); - ret = executor.spin_until_future_complete(future, 1s); - EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); - - // test timeout - future = std::async( - []() { - std::this_thread::sleep_for(1s); - return; - }); - ret = executor.spin_until_future_complete(future, 100ms); - EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); -} - -// Make sure that the spin_until_future_complete works correctly with std::shared_future -TEST_F(TestExecutors, testSpinUntilFutureCompleteSharedFuture) { - rclcpp::executors::SingleThreadedExecutor executor; - std::future future; - rclcpp::FutureReturnCode ret; - - // test success - future = std::async( - []() { - return; - }); - ret = executor.spin_until_future_complete(future.share(), 1s); - EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); - - // test timeout - future = std::async( - []() { - std::this_thread::sleep_for(1s); - return; - }); - ret = executor.spin_until_future_complete(future.share(), 100ms); - EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); -} From 6205eccc9f689565978c2e80ee56af80affbbad1 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Wed, 10 Jun 2020 03:40:17 -0400 Subject: [PATCH 089/121] Fix reference to rclcpp in its QD (#1161) Signed-off-by: Christophe Bedard --- rclcpp/QUALITY_DECLARATION.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/QUALITY_DECLARATION.md b/rclcpp/QUALITY_DECLARATION.md index 8b2b970887..83e024f20c 100644 --- a/rclcpp/QUALITY_DECLARATION.md +++ b/rclcpp/QUALITY_DECLARATION.md @@ -191,7 +191,7 @@ It is **Quality Level 4**, see its [Quality Declaration document](https://github #### `tracetools` -The `tracetools` package provides utilities for instrumenting the code in `rcl` so that it may be traced for debugging and performance analysis. +The `tracetools` package provides utilities for instrumenting the code in `rclcpp` so that it may be traced for debugging and performance analysis. It is **Quality Level 4**, see its [Quality Declaration document](https://gitlab.com/micro-ROS/ros_tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md). From 7c616a1d41912475ce4fc65761f1532ddb4370ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 10 Jun 2020 17:03:09 +0200 Subject: [PATCH 090/121] Fixed doxygen warnings (#1163) Signed-off-by: ahcorde --- rclcpp/include/rclcpp/clock.hpp | 6 ++-- .../executors/multi_threaded_executor.hpp | 1 + rclcpp/include/rclcpp/node.hpp | 7 +++-- rclcpp/include/rclcpp/parameter_client.hpp | 30 +++++++++---------- rclcpp/include/rclcpp/serialization.hpp | 4 +-- rclcpp/include/rclcpp/service.hpp | 4 +-- .../include/rclcpp/subscription_factory.hpp | 2 +- rclcpp/include/rclcpp/time.hpp | 5 ++-- rclcpp/include/rclcpp/timer.hpp | 1 + .../subscription_topic_statistics.hpp | 2 +- rclcpp/include/rclcpp/wait_set_template.hpp | 2 ++ rclcpp/include/rclcpp/waitable.hpp | 2 +- .../include/rclcpp_action/create_client.hpp | 8 ++--- .../include/rclcpp_action/create_server.hpp | 18 +++++------ .../rclcpp_lifecycle/lifecycle_node.hpp | 1 - 15 files changed, 48 insertions(+), 45 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 211966ee41..45604cf1bc 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -115,9 +115,9 @@ class Clock * * Function is only applicable if the clock_type is `RCL_ROS_TIME` * - * \param pre_callback. Must be non-throwing - * \param post_callback. Must be non-throwing. - * \param threshold. Callbacks will be triggered if the time jump is greater + * \param pre_callback Must be non-throwing + * \param post_callback Must be non-throwing. + * \param threshold Callbacks will be triggered if the time jump is greater * then the threshold. * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. * \throws std::bad_alloc if the allocation of the JumpHandler fails. diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 062b6fbbb7..c18df5b7bc 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -49,6 +49,7 @@ class MultiThreadedExecutor : public rclcpp::Executor * \param number_of_threads number of threads to have in the thread pool, * the default 0 will use the number of cpu cores found instead * \param yield_before_execute if true std::this_thread::yield() is called + * \param timeout maximum time to wait */ RCLCPP_PUBLIC MultiThreadedExecutor( diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index f379367d2e..7706fa8a66 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -235,7 +235,7 @@ class Node : public std::enable_shared_from_this /// Create and return a Client. /** * \param[in] service_name The topic to service on. - * \param[in] rmw_qos_profile_t Quality of service profile for client. + * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. * \param[in] group Callback group to call the service. * \return Shared pointer to the created client. */ @@ -250,7 +250,7 @@ class Node : public std::enable_shared_from_this /** * \param[in] service_name The topic to service on. * \param[in] callback User-defined callback function. - * \param[in] rmw_qos_profile_t Quality of service profile for client. + * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. * \param[in] group Callback group to call the service. * \return Shared pointer to the created service. */ @@ -891,7 +891,8 @@ class Node : public std::enable_shared_from_this /// Return the number of publishers that are advertised on a given topic. /** - * \param[in] topic_name the topic_name on which to count the publishers. + * \param[in] node_name the node_name on which to count the publishers. + * \param[in] namespace_ the namespace of the node associated with the name * \return number of publishers that are advertised on a given topic. * \throws std::runtime_error if publishers could not be counted */ diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index fa45dd285e..0305529809 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -48,13 +48,13 @@ class AsyncParametersClient /// Create an async parameters client. /** - * \param node_base_interface[in] The node base interface of the corresponding node. - * \param node_topics_interface[in] Node topic base interface. - * \param node_graph_interface[in] The node graph interface of the corresponding node. - * \param node_services_interface[in] Node service interface. - * \param remote_node_name[in] (optional) name of the remote node - * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe - * \param group[in] (optional) The async parameter client will be added to this callback group. + * \param[in] node_base_interface The node base interface of the corresponding node. + * \param[in] node_topics_interface Node topic base interface. + * \param[in] node_graph_interface The node graph interface of the corresponding node. + * \param[in] node_services_interface Node service interface. + * \param[in] remote_node_name (optional) name of the remote node + * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. */ RCLCPP_PUBLIC AsyncParametersClient( @@ -68,10 +68,10 @@ class AsyncParametersClient /// Constructor /** - * \param node[in] The async paramters client will be added to this node. - * \param remote_node_name[in] (optional) name of the remote node - * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe - * \param group[in] (optional) The async parameter client will be added to this callback group. + * \param[in] node The async paramters client will be added to this node. + * \param[in] remote_node_name (optional) name of the remote node + * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. */ RCLCPP_PUBLIC AsyncParametersClient( @@ -82,10 +82,10 @@ class AsyncParametersClient /// Constructor /** - * \param node[in] The async paramters client will be added to this node. - * \param remote_node_name[in] (optional) name of the remote node - * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe - * \param group[in] (optional) The async parameter client will be added to this callback group. + * \param[in] node The async paramters client will be added to this node. + * \param[in] remote_node_name (optional) name of the remote node + * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. */ RCLCPP_PUBLIC AsyncParametersClient( diff --git a/rclcpp/include/rclcpp/serialization.hpp b/rclcpp/include/rclcpp/serialization.hpp index 787d18c523..09e92fa70a 100644 --- a/rclcpp/include/rclcpp/serialization.hpp +++ b/rclcpp/include/rclcpp/serialization.hpp @@ -86,7 +86,7 @@ class RCLCPP_PUBLIC_TYPE SerializationBase /// Serialize a ROS2 message to a serialized stream /** - * \param[in] message The ROS2 message which is read and serialized by rmw. + * \param[in] ros_message The ROS2 message which is read and serialized by rmw. * \param[out] serialized_message The serialized message. */ void serialize_message( @@ -95,7 +95,7 @@ class RCLCPP_PUBLIC_TYPE SerializationBase /// Deserialize a serialized stream to a ROS message /** * \param[in] serialized_message The serialized message to be converted to ROS2 by rmw. - * \param[out] message The deserialized ROS2 message. + * \param[out] ros_message The deserialized ROS2 message. */ void deserialize_message( const SerializedMessage * serialized_message, void * ros_message) const; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 2ba6b7f6a3..094b628987 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -237,7 +237,7 @@ class Service : public ServiceBase * rclcpp::create_service(). * * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. - * \param[in] service_name Name of the topic to publish to. + * \param[in] service_handle service handle. * \param[in] any_callback User defined callback to call when a client request is received. */ Service( @@ -272,7 +272,7 @@ class Service : public ServiceBase * rclcpp::create_service(). * * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. - * \param[in] service_name Name of the topic to publish to. + * \param[in] service_handle service handle. * \param[in] any_callback User defined callback to call when a client request is received. */ Service( diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index fc3933ebcf..694be60a74 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -68,7 +68,7 @@ struct SubscriptionFactory * \param[in] callback The user-defined callback function to receive a message * \param[in] options Additional options for the creation of the Subscription. * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. - * \param[in] subscription_topic_Optinal stats callback for topic_statistics + * \param[in] subscription_topic_stats Optional stats callback for topic_statistics */ template< typename MessageT, diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index 64c6126316..6782393b4c 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -48,7 +48,7 @@ class Time /// Time constructor /** * \param nanoseconds since time epoch - * \param clock_type clock type + * \param clock clock type */ RCLCPP_PUBLIC explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME); @@ -60,7 +60,7 @@ class Time /// Time constructor /** * \param time_msg builtin_interfaces time message to copy - * \param clock_type clock type + * \param ros_time clock type * \throws std::runtime_error if seconds are negative */ RCLCPP_PUBLIC @@ -71,7 +71,6 @@ class Time /// Time constructor /** * \param time_point rcl_time_point_t structure to copy - * \param clock_type clock type */ RCLCPP_PUBLIC explicit Time(const rcl_time_point_t & time_point); diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 66a7529bce..6f0a0ca88e 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -166,6 +166,7 @@ class GenericTimer : public TimerBase * \param[in] clock The clock providing the current time. * \param[in] period The interval at which the timer fires. * \param[in] callback User-specified callback function. + * \param[in] context custom context to be used. */ explicit GenericTimer( Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback, diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index e450aa5d65..46155ca4e9 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -116,7 +116,7 @@ class SubscriptionTopicStatistics /// Set the timer used to publish statistics messages. /** - * \param measurement_timer the timer to fire the publisher, created by the node + * \param publisher_timer the timer to fire the publisher, created by the node */ void set_publisher_timer(rclcpp::TimerBase::SharedPtr publisher_timer) { diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 2ab6f57ba1..87e3974ac6 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -61,6 +61,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli * \param[in] subscriptions Vector of subscriptions to be added. * \param[in] guard_conditions Vector of guard conditions to be added. * \param[in] timers Vector of timers to be added. + * \param[in] clients Vector of clients and their associated entity to be added. + * \param[in] services Vector of services and their associated entity to be added. * \param[in] waitables Vector of waitables and their associated entity to be added. * \param[in] context Custom context to be used, defaults to global default. * \throws std::invalid_argument If context is nullptr. diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 428cc1d660..64504f3950 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -123,7 +123,7 @@ class Waitable RCLCPP_PUBLIC virtual bool - is_ready(rcl_wait_set_t *) = 0; + is_ready(rcl_wait_set_t * wait_set) = 0; /// Execute any entities of the Waitable that are ready. /** diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 2c4d495ea4..7c3f210cb6 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -30,10 +30,10 @@ namespace rclcpp_action * This function is equivalent to \sa create_client()` however is using the individual * node interfaces to create the client. * - * \param node_base_interface[in] The node base interface of the corresponding node. - * \param node_graph_interface[in] The node graph interface of the corresponding node. - * \param node_logging_interface[in] The node logging interface of the corresponding node. - * \param node_waitables_interface[in] The node waitables interface of the corresponding node. + * \param[in] node_base_interface The node base interface of the corresponding node. + * \param[in] node_graph_interface The node graph interface of the corresponding node. + * \param[in] node_logging_interface The node logging interface of the corresponding node. + * \param[in] node_waitables_interface The node waitables interface of the corresponding node. * \param[in] name The action name. * \param[in] group The action client will be added to this callback group. * If `nullptr`, then the action client is added to the default callback group. diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index b579fea800..189bc710a4 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -39,18 +39,18 @@ namespace rclcpp_action * * \sa Server::Server() for more information. * - * \param node_base_interface[in] The node base interface of the corresponding node. - * \param node_clock_interface[in] The node clock interface of the corresponding node. - * \param node_logging_interface[in] The node logging interface of the corresponding node. - * \param node_waitables_interface[in] The node waitables interface of the corresponding node. - * \param name[in] The action name. + * \param[in] node_base_interface The node base interface of the corresponding node. + * \param[in] node_clock_interface The node clock interface of the corresponding node. + * \param[in] node_logging_interface The node logging interface of the corresponding node. + * \param[in] node_waitables_interface The node waitables interface of the corresponding node. + * \param[in] name The action name. * \param[in] handle_goal A callback that decides if a goal should be accepted or rejected. * \param[in] handle_cancel A callback that decides if a goal should be attempted to be canceled. * The return from this callback only indicates if the server will try to cancel a goal. * It does not indicate if the goal was actually canceled. * \param[in] handle_accepted A callback that is called to give the user a handle to the goal. * \param[in] options options to pass to the underlying `rcl_action_server_t`. - * \param group[in] The action server will be added to this callback group. + * \param[in] group The action server will be added to this callback group. * If `nullptr`, then the action server is added to the default callback group. */ template @@ -116,15 +116,15 @@ create_server( * * \sa Server::Server() for more information. * - * \param node[in] The action server will be added to this node. - * \param name[in] The action name. + * \param[in] node] The action server will be added to this node. + * \param[in] name The action name. * \param[in] handle_goal A callback that decides if a goal should be accepted or rejected. * \param[in] handle_cancel A callback that decides if a goal should be attempted to be canceled. * The return from this callback only indicates if the server will try to cancel a goal. * It does not indicate if the goal was actually canceled. * \param[in] handle_accepted A callback that is called to give the user a handle to the goal. * \param[in] options options to pass to the underlying `rcl_action_server_t`. - * \param group[in] The action server will be added to this callback group. + * \param[in] group The action server will be added to this callback group. * If `nullptr`, then the action server is added to the default callback group. */ template diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 5c234e8f2d..bd2327fb97 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -124,7 +124,6 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, /// Create a new lifecycle node with the specified name. /** * \param[in] node_name Name of the node. - * \param[in] namespace_ Namespace of the node. * \param[in] options Additional options to control creation of the node. */ RCLCPP_LIFECYCLE_PUBLIC From 7ef258fff7a3c44c63c7a5cdea585b0a99ffa9c3 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 10 Jun 2020 13:39:38 -0300 Subject: [PATCH 091/121] Revert "Revert "Allow spin_until_future_complete to accept std::future (#1113)" (#1159)" (#1160) This reverts commit bba9dce253d98a31a3069218504d26c4be9fb8b8. Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/executor.hpp | 4 +- rclcpp/include/rclcpp/executors.hpp | 12 ++--- .../static_single_threaded_executor.hpp | 4 +- rclcpp/test/test_executor.cpp | 48 +++++++++++++++++++ 4 files changed, 58 insertions(+), 10 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 39acba3ec4..77fb35ca2e 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -188,10 +188,10 @@ class Executor * code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ - template + template FutureReturnCode spin_until_future_complete( - const std::shared_future & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index ec55d1309e..36fb0d63cf 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -66,12 +66,12 @@ using rclcpp::executors::SingleThreadedExecutor; * If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ -template +template rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const std::shared_future & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete @@ -82,13 +82,13 @@ spin_node_until_future_complete( return retcode; } -template rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, std::shared_ptr node_ptr, - const std::shared_future & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::executors::spin_node_until_future_complete( @@ -104,7 +104,7 @@ template & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { rclcpp::executors::SingleThreadedExecutor executor; @@ -116,7 +116,7 @@ template node_ptr, - const std::shared_future & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 1e1668bcb3..25645e23f2 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -143,10 +143,10 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor * exec.add_node(node); * exec.spin_until_future_complete(future); */ - template + template rclcpp::FutureReturnCode spin_until_future_complete( - std::shared_future & future, + FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { std::future_status status = future.wait_for(std::chrono::seconds(0)); diff --git a/rclcpp/test/test_executor.cpp b/rclcpp/test/test_executor.cpp index 95371415fc..cdefec71e1 100644 --- a/rclcpp/test/test_executor.cpp +++ b/rclcpp/test/test_executor.cpp @@ -67,3 +67,51 @@ TEST_F(TestExecutors, addTemporaryNode) { executor.add_node(std::make_shared("temporary_node")); EXPECT_NO_THROW(executor.spin_some()); } + +// Make sure that the spin_until_future_complete works correctly with std::future +TEST_F(TestExecutors, testSpinUntilFutureComplete) { + rclcpp::executors::SingleThreadedExecutor executor; + std::future future; + rclcpp::FutureReturnCode ret; + + // test success + future = std::async( + []() { + return; + }); + ret = executor.spin_until_future_complete(future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + + // test timeout + future = std::async( + []() { + std::this_thread::sleep_for(1s); + return; + }); + ret = executor.spin_until_future_complete(future, 100ms); + EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); +} + +// Make sure that the spin_until_future_complete works correctly with std::shared_future +TEST_F(TestExecutors, testSpinUntilFutureCompleteSharedFuture) { + rclcpp::executors::SingleThreadedExecutor executor; + std::future future; + rclcpp::FutureReturnCode ret; + + // test success + future = std::async( + []() { + return; + }); + ret = executor.spin_until_future_complete(future.share(), 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + + // test timeout + future = std::async( + []() { + std::this_thread::sleep_for(1s); + return; + }); + ret = executor.spin_until_future_complete(future.share(), 100ms); + EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); +} From 3afe623bec8948e2541e21395b7b40542362bb1e Mon Sep 17 00:00:00 2001 From: DongheeYe <45094526+DongheeYe@users.noreply.github.com> Date: Fri, 12 Jun 2020 02:25:02 +0900 Subject: [PATCH 092/121] Fix spin_until_future_complete: check spinning value (#1023) Signed-off-by: Donghee Ye --- rclcpp/include/rclcpp/executor.hpp | 14 ++++++++++++-- rclcpp/src/rclcpp/executor.cpp | 14 ++++++++++---- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 77fb35ca2e..557e4f3ab3 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -37,6 +37,7 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/scope_exit.hpp" namespace rclcpp { @@ -212,9 +213,14 @@ class Executor } std::chrono::nanoseconds timeout_left = timeout_ns; - while (rclcpp::ok(this->context_)) { + if (spinning.exchange(true)) { + throw std::runtime_error("spin_until_future_complete() called while already spinning"); + } + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + while (rclcpp::ok(this->context_) && spinning.load()) { // Do one item of work. - spin_once(timeout_left); + spin_once_impl(timeout_left); + // Check if the future is set, return SUCCESS if it is. status = future.wait_for(std::chrono::seconds(0)); if (status == std::future_status::ready) { @@ -336,6 +342,10 @@ class Executor RCLCPP_DISABLE_COPY(Executor) + RCLCPP_PUBLIC + void + spin_once_impl(std::chrono::nanoseconds timeout); + std::list weak_nodes_; std::list guard_conditions_; }; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 878628d901..83a8baf5bd 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -244,6 +244,15 @@ Executor::spin_some(std::chrono::nanoseconds max_duration) } } +void +Executor::spin_once_impl(std::chrono::nanoseconds timeout) +{ + AnyExecutable any_exec; + if (get_next_executable(any_exec, timeout)) { + execute_any_executable(any_exec); + } +} + void Executor::spin_once(std::chrono::nanoseconds timeout) { @@ -251,10 +260,7 @@ Executor::spin_once(std::chrono::nanoseconds timeout) throw std::runtime_error("spin_once() called while already spinning"); } RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); - AnyExecutable any_exec; - if (get_next_executable(any_exec, timeout)) { - execute_any_executable(any_exec); - } + spin_once_impl(timeout); } void From f815d8981b57c9f256ac6d08789cb8a48dddc930 Mon Sep 17 00:00:00 2001 From: Devin Bonnie <47613035+dabonnie@users.noreply.github.com> Date: Thu, 11 Jun 2020 11:08:38 -0700 Subject: [PATCH 093/121] Add check for invalid topic statistics publish period (#1151) * Add check for invalid topic statistics publish period Signed-off-by: Devin Bonnie * Update documentation Signed-off-by: Devin Bonnie * Address review comments Signed-off-by: Devin Bonnie * Address doc formatting comments Signed-off-by: Devin Bonnie * Update doc spacing Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/create_subscription.hpp | 25 +++++++++++++++++++ .../include/rclcpp/subscription_options.hpp | 3 ++- .../test_subscription_topic_statistics.cpp | 25 +++++++++++++++++++ 3 files changed, 52 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 207bee4a71..2eb24b48b5 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -44,6 +45,23 @@ namespace rclcpp * The NodeT type only needs to have a method called get_node_topics_interface() * which returns a shared_ptr to a NodeTopicsInterface, or be a * NodeTopicsInterface pointer itself. + * + * \tparam MessageT + * \tparam CallbackT + * \tparam AllocatorT + * \tparam CallbackMessageT + * \tparam SubscriptionT + * \tparam MessageMemoryStrategyT + * \tparam NodeT + * \param node + * \param topic_name + * \param qos + * \param callback + * \param options + * \param msg_mem_strat + * \return the created subscription + * \throws std::invalid_argument if topic statistics is enabled and the publish period is + * less than or equal to zero. */ template< typename MessageT, @@ -81,6 +99,13 @@ create_subscription( options, *node_topics->get_node_base_interface())) { + if (options.topic_stats_options.publish_period <= std::chrono::milliseconds(0)) { + throw std::invalid_argument( + "topic_stats_options.publish_period must be greater than 0, specified value of " + + std::to_string(options.topic_stats_options.publish_period.count()) + + " ms"); + } + std::shared_ptr> publisher = create_publisher( node, diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 5088f35fb1..ebf4331c4f 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -66,7 +66,8 @@ struct SubscriptionOptionsBase // Topic to which topic statistics get published when enabled. Defaults to /statistics. std::string publish_topic = "/statistics"; - // Topic statistics publication period in ms. Defaults to one minute. + // Topic statistics publication period in ms. Defaults to one second. + // Only values greater than zero are allowed. std::chrono::milliseconds publish_period{std::chrono::seconds(1)}; }; diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 74a79e6f9b..dc4e1fa86e 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -219,6 +220,30 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test } }; +TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period) +{ + rclcpp::init(0 /* argc */, nullptr /* argv */); + + auto node = std::make_shared("test_period_node"); + + auto options = rclcpp::SubscriptionOptions(); + options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; + options.topic_stats_options.publish_period = std::chrono::milliseconds(0); + + auto callback = [](Empty::UniquePtr msg) { + (void) msg; + }; + + ASSERT_THROW( + (node->create_subscription>( + "should_throw_invalid_arg", + rclcpp::QoS(rclcpp::KeepAll()), + callback, + options)), std::invalid_argument); + + rclcpp::shutdown(); +} + TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) { auto empty_subscriber = std::make_shared( From 7b7e5e0718f9bc9030f8581b1b2d65c1cf898d65 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 11 Jun 2020 17:17:39 -0300 Subject: [PATCH 094/121] Check if context is valid when looping in spin_some (#1167) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/executor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 83a8baf5bd..0c83e0deba 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -234,7 +234,7 @@ Executor::spin_some(std::chrono::nanoseconds max_duration) RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); // non-blocking call to pre-load all available work wait_for_work(std::chrono::milliseconds::zero()); - while (spinning.load() && max_duration_not_elapsed()) { + while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { AnyExecutable any_exec; if (get_next_ready_executable(any_exec)) { execute_any_executable(any_exec); From b916f5d67b21fffbbacdee9e4a90aa6455868430 Mon Sep 17 00:00:00 2001 From: brawner Date: Fri, 12 Jun 2020 11:40:15 -0700 Subject: [PATCH 095/121] Reorganize test directory and split CMakeLists.txt (#1173) Signed-off-by: Stephen Brawner --- rclcpp/CMakeLists.txt | 434 +----------------- rclcpp/test/CMakeLists.txt | 434 ++++++++++++++++++ .../cmake/rclcpp_add_build_failure_test.cmake | 0 .../test_multi_threaded_executor.cpp | 0 .../node_interfaces/node_wrapper.hpp | 6 +- ...topics_interface_const_ptr_rclcpp_node.cpp | 0 ...opics_interface_const_ptr_wrapped_node.cpp | 0 ...topics_interface_const_ref_rclcpp_node.cpp | 0 ...opics_interface_const_ref_wrapped_node.cpp | 0 .../test_get_node_interfaces.cpp | 0 rclcpp/test/{ => rclcpp}/test_client.cpp | 0 .../test/{ => rclcpp}/test_create_timer.cpp | 0 rclcpp/test/{ => rclcpp}/test_duration.cpp | 0 rclcpp/test/{ => rclcpp}/test_executor.cpp | 0 .../test_expand_topic_or_service_name.cpp | 0 .../test_externally_defined_services.cpp | 0 .../{ => rclcpp}/test_find_weak_nodes.cpp | 0 .../{ => rclcpp}/test_function_traits.cpp | 0 .../{ => rclcpp}/test_guard_condition.cpp | 0 rclcpp/test/{ => rclcpp}/test_init.cpp | 0 .../{ => rclcpp}/test_interface_traits.cpp | 0 .../test_intra_process_buffer.cpp | 0 .../test_intra_process_manager.cpp | 0 .../test/{ => rclcpp}/test_loaned_message.cpp | 0 rclcpp/test/{ => rclcpp}/test_logger.cpp | 0 rclcpp/test/{ => rclcpp}/test_logging.cpp | 0 rclcpp/test/{ => rclcpp}/test_node.cpp | 0 .../{ => rclcpp}/test_node_global_args.cpp | 0 .../test/{ => rclcpp}/test_node_options.cpp | 0 rclcpp/test/{ => rclcpp}/test_parameter.cpp | 0 .../{ => rclcpp}/test_parameter_client.cpp | 0 .../test_parameter_events_filter.cpp | 0 .../test/{ => rclcpp}/test_parameter_map.cpp | 0 rclcpp/test/{ => rclcpp}/test_publisher.cpp | 0 .../test_publisher_subscription_count_api.cpp | 0 rclcpp/test/{ => rclcpp}/test_qos.cpp | 0 rclcpp/test/{ => rclcpp}/test_qos_event.cpp | 0 rclcpp/test/{ => rclcpp}/test_rate.cpp | 0 .../test_ring_buffer_implementation.cpp | 0 .../{ => rclcpp}/test_serialized_message.cpp | 0 .../test_serialized_message_allocator.cpp | 0 rclcpp/test/{ => rclcpp}/test_service.cpp | 0 .../test/{ => rclcpp}/test_subscription.cpp | 0 .../test_subscription_options.cpp | 0 .../test_subscription_publisher_count_api.cpp | 0 .../{ => rclcpp}/test_subscription_traits.cpp | 0 rclcpp/test/{ => rclcpp}/test_time.cpp | 0 rclcpp/test/{ => rclcpp}/test_time_source.cpp | 0 rclcpp/test/{ => rclcpp}/test_timer.cpp | 0 rclcpp/test/{ => rclcpp}/test_utilities.cpp | 0 rclcpp/test/{ => rclcpp}/test_wait_set.cpp | 0 .../test_subscription_topic_statistics.cpp | 0 .../test_topic_stats_utils.hpp | 6 +- 53 files changed, 441 insertions(+), 439 deletions(-) create mode 100644 rclcpp/test/CMakeLists.txt rename rclcpp/{ => test}/cmake/rclcpp_add_build_failure_test.cmake (100%) rename rclcpp/test/{ => rclcpp}/executors/test_multi_threaded_executor.cpp (100%) rename rclcpp/test/{ => rclcpp}/node_interfaces/node_wrapper.hpp (93%) rename rclcpp/test/{ => rclcpp}/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp (100%) rename rclcpp/test/{ => rclcpp}/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp (100%) rename rclcpp/test/{ => rclcpp}/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp (100%) rename rclcpp/test/{ => rclcpp}/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp (100%) rename rclcpp/test/{ => rclcpp}/node_interfaces/test_get_node_interfaces.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_client.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_create_timer.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_duration.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_executor.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_expand_topic_or_service_name.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_externally_defined_services.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_find_weak_nodes.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_function_traits.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_guard_condition.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_init.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_interface_traits.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_intra_process_buffer.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_intra_process_manager.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_loaned_message.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_logger.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_logging.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_node.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_node_global_args.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_node_options.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_parameter.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_parameter_client.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_parameter_events_filter.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_parameter_map.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_publisher.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_publisher_subscription_count_api.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_qos.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_qos_event.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_rate.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_ring_buffer_implementation.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_serialized_message.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_serialized_message_allocator.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_service.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_subscription.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_subscription_options.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_subscription_publisher_count_api.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_subscription_traits.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_time.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_time_source.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_timer.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_utilities.cpp (100%) rename rclcpp/test/{ => rclcpp}/test_wait_set.cpp (100%) rename rclcpp/test/{ => rclcpp}/topic_statistics/test_subscription_topic_statistics.cpp (100%) rename rclcpp/test/{ => rclcpp}/topic_statistics/test_topic_stats_utils.hpp (95%) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 4562375a15..81db9a365d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -212,442 +212,10 @@ ament_export_dependencies(statistics_msgs) ament_export_dependencies(tracetools) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - find_package(rmw_implementation_cmake REQUIRED) - find_package(rosidl_default_generators REQUIRED) - - find_package(test_msgs REQUIRED) - - include(cmake/rclcpp_add_build_failure_test.cmake) - - add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/test/resources") - - rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs - test/msg/Header.msg - test/msg/MessageWithHeader.msg - DEPENDENCIES builtin_interfaces - LIBRARY_NAME ${PROJECT_NAME} - SKIP_INSTALL - ) - - ament_add_gtest(test_client test/test_client.cpp) - if(TARGET test_client) - ament_target_dependencies(test_client - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_client ${PROJECT_NAME}) - endif() - ament_add_gtest(test_create_timer test/test_create_timer.cpp) - if(TARGET test_create_timer) - ament_target_dependencies(test_create_timer - "rcl_interfaces" - "rmw" - "rcl" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_create_timer ${PROJECT_NAME}) - target_include_directories(test_create_timer PRIVATE test/) - endif() - ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp) - if(TARGET test_expand_topic_or_service_name) - ament_target_dependencies(test_expand_topic_or_service_name - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME}) - endif() - ament_add_gtest(test_function_traits test/test_function_traits.cpp) - if(TARGET test_function_traits) - target_include_directories(test_function_traits PUBLIC include) - ament_target_dependencies(test_function_traits - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - endif() - ament_add_gmock(test_intra_process_manager test/test_intra_process_manager.cpp) - if(TARGET test_intra_process_manager) - ament_target_dependencies(test_intra_process_manager - "rcl" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) - endif() - ament_add_gtest(test_ring_buffer_implementation test/test_ring_buffer_implementation.cpp) - if(TARGET test_ring_buffer_implementation) - ament_target_dependencies(test_ring_buffer_implementation - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME}) - endif() - ament_add_gtest(test_intra_process_buffer test/test_intra_process_buffer.cpp) - if(TARGET test_intra_process_buffer) - ament_target_dependencies(test_intra_process_buffer - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_intra_process_buffer ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_loaned_message test/test_loaned_message.cpp) - ament_target_dependencies(test_loaned_message - "test_msgs" - ) - target_link_libraries(test_loaned_message ${PROJECT_NAME}) - - ament_add_gtest(test_node test/test_node.cpp TIMEOUT 240) - if(TARGET test_node) - ament_target_dependencies(test_node - "rcl_interfaces" - "rcpputils" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_node ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_node_interfaces__get_node_interfaces - test/node_interfaces/test_get_node_interfaces.cpp) - if(TARGET test_node_interfaces__get_node_interfaces) - target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME}) - endif() - - # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output - # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node - # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp) - # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node - # ${PROJECT_NAME}) - - # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node - # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp) - # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node - # ${PROJECT_NAME}) - - # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node - # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp) - # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node - # ${PROJECT_NAME}) - - # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node - # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp) - # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node - # ${PROJECT_NAME}) - - ament_add_gtest(test_node_global_args test/test_node_global_args.cpp) - if(TARGET test_node_global_args) - ament_target_dependencies(test_node_global_args - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_node_global_args ${PROJECT_NAME}) - endif() - ament_add_gtest(test_node_options test/test_node_options.cpp) - if(TARGET test_node_options) - ament_target_dependencies(test_node_options "rcl") - target_link_libraries(test_node_options ${PROJECT_NAME}) - endif() - ament_add_gtest(test_parameter_client test/test_parameter_client.cpp) - if(TARGET test_parameter_client) - ament_target_dependencies(test_parameter_client - "rcl_interfaces" - ) - target_link_libraries(test_parameter_client ${PROJECT_NAME}) - endif() - ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp) - if(TARGET test_parameter_events_filter) - ament_target_dependencies(test_parameter_events_filter - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_parameter_events_filter ${PROJECT_NAME}) - endif() - ament_add_gtest(test_parameter test/test_parameter.cpp) - if(TARGET test_parameter) - ament_target_dependencies(test_parameter - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_parameter ${PROJECT_NAME}) - endif() - ament_add_gtest(test_parameter_map test/test_parameter_map.cpp) - if(TARGET test_parameter_map) - target_link_libraries(test_parameter_map ${PROJECT_NAME}) - endif() - ament_add_gtest(test_publisher test/test_publisher.cpp) - if(TARGET test_publisher) - ament_target_dependencies(test_publisher - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_publisher ${PROJECT_NAME}) - endif() - ament_add_gtest(test_publisher_subscription_count_api test/test_publisher_subscription_count_api.cpp) - if(TARGET test_publisher_subscription_count_api) - ament_target_dependencies(test_publisher_subscription_count_api - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME}) - endif() - ament_add_gtest(test_qos test/test_qos.cpp) - if(TARGET test_qos) - ament_target_dependencies(test_qos - "rmw" - ) - target_link_libraries(test_qos - ${PROJECT_NAME} - ) - endif() - ament_add_gtest(test_qos_event test/test_qos_event.cpp) - if(TARGET test_qos_event) - ament_target_dependencies(test_qos_event - "rmw" - "test_msgs" - ) - target_link_libraries(test_qos_event - ${PROJECT_NAME} - ) - endif() - ament_add_gtest(test_rate test/test_rate.cpp) - if(TARGET test_rate) - ament_target_dependencies(test_rate - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_rate - ${PROJECT_NAME} - ) - endif() - ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp) - if(TARGET test_serialized_message_allocator) - ament_target_dependencies(test_serialized_message_allocator - test_msgs - ) - target_link_libraries(test_serialized_message_allocator - ${PROJECT_NAME} - ) - endif() - ament_add_gtest(test_serialized_message test/test_serialized_message.cpp) - if(TARGET test_serialized_message) - ament_target_dependencies(test_serialized_message - test_msgs - ) - target_link_libraries(test_serialized_message - ${PROJECT_NAME} - ) - endif() - ament_add_gtest(test_service test/test_service.cpp) - if(TARGET test_service) - ament_target_dependencies(test_service - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_service ${PROJECT_NAME}) - endif() - ament_add_gtest(test_subscription test/test_subscription.cpp) - if(TARGET test_subscription) - ament_target_dependencies(test_subscription - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_subscription ${PROJECT_NAME}) - endif() - ament_add_gtest(test_subscription_publisher_count_api test/test_subscription_publisher_count_api.cpp) - if(TARGET test_subscription_publisher_count_api) - ament_target_dependencies(test_subscription_publisher_count_api - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME}) - endif() - ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp) - if(TARGET test_subscription_traits) - ament_target_dependencies(test_subscription_traits - "rcl" - "test_msgs" - ) - target_link_libraries(test_subscription_traits ${PROJECT_NAME}) - endif() - ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp) - if(TARGET test_find_weak_nodes) - ament_target_dependencies(test_find_weak_nodes - "rcl" - ) - target_link_libraries(test_find_weak_nodes ${PROJECT_NAME}) - endif() - - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") - if(WIN32) - set(append_library_dirs "${append_library_dirs}/$") - endif() - - ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp) - ament_target_dependencies(test_externally_defined_services - "rcl" - "test_msgs" - ) - target_link_libraries(test_externally_defined_services ${PROJECT_NAME}) - - ament_add_gtest(test_duration test/test_duration.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_duration) - ament_target_dependencies(test_duration - "rcl") - target_link_libraries(test_duration ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_executor test/test_executor.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_executor) - ament_target_dependencies(test_executor - "rcl") - target_link_libraries(test_executor ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_logger test/test_logger.cpp) - target_link_libraries(test_logger ${PROJECT_NAME}) - - ament_add_gmock(test_logging test/test_logging.cpp) - target_link_libraries(test_logging ${PROJECT_NAME}) - - ament_add_gtest(test_time test/test_time.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_time) - ament_target_dependencies(test_time - "rcl") - target_link_libraries(test_time ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_timer test/test_timer.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_timer) - ament_target_dependencies(test_timer - "rcl") - target_link_libraries(test_timer ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_time_source test/test_time_source.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_time_source) - ament_target_dependencies(test_time_source - "rcl") - target_link_libraries(test_time_source ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_utilities test/test_utilities.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_utilities) - ament_target_dependencies(test_utilities - "rcl") - target_link_libraries(test_utilities ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_init test/test_init.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_init) - ament_target_dependencies(test_init - "rcl") - target_link_libraries(test_init ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_interface_traits test/test_interface_traits.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_interface_traits) - ament_target_dependencies(test_interface_traits - "rcl") - target_link_libraries(test_interface_traits ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_multi_threaded_executor) - ament_target_dependencies(test_multi_threaded_executor - "rcl") - target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_guard_condition test/test_guard_condition.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_guard_condition) - target_link_libraries(test_guard_condition ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_wait_set test/test_wait_set.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_wait_set) - ament_target_dependencies(test_wait_set "test_msgs") - target_link_libraries(test_wait_set ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}" - ) - if(TARGET test_subscription_topic_statistics) - ament_target_dependencies(test_subscription_topic_statistics - "builtin_interfaces" - "libstatistics_collector" - "rcl_interfaces" - "rcutils" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "statistics_msgs" - "test_msgs") - rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") - target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_subscription_options test/test_subscription_options.cpp) - if(TARGET test_subscription_options) - ament_target_dependencies(test_subscription_options "rcl") - target_link_libraries(test_subscription_options ${PROJECT_NAME}) - endif() - - # Install test resources - install( - DIRECTORY test/resources - DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test) + add_subdirectory(test) endif() ament_package() diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt new file mode 100644 index 0000000000..d500f3e58a --- /dev/null +++ b/rclcpp/test/CMakeLists.txt @@ -0,0 +1,434 @@ +find_package(ament_cmake_gtest REQUIRED) + +find_package(rmw_implementation_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +find_package(test_msgs REQUIRED) + +include(cmake/rclcpp_add_build_failure_test.cmake) + +add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/resources") + +rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs + msg/Header.msg + msg/MessageWithHeader.msg + DEPENDENCIES builtin_interfaces + LIBRARY_NAME ${PROJECT_NAME} + SKIP_INSTALL +) + +ament_add_gtest(test_client rclcpp/test_client.cpp) +if(TARGET test_client) + ament_target_dependencies(test_client + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_client ${PROJECT_NAME}) +endif() +ament_add_gtest(test_create_timer rclcpp/test_create_timer.cpp) +if(TARGET test_create_timer) + ament_target_dependencies(test_create_timer + "rcl_interfaces" + "rmw" + "rcl" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_create_timer ${PROJECT_NAME}) + target_include_directories(test_create_timer PRIVATE rclcpp/) +endif() +ament_add_gtest(test_expand_topic_or_service_name rclcpp/test_expand_topic_or_service_name.cpp) +if(TARGET test_expand_topic_or_service_name) + ament_target_dependencies(test_expand_topic_or_service_name + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME}) +endif() +ament_add_gtest(test_function_traits rclcpp/test_function_traits.cpp) +if(TARGET test_function_traits) + target_include_directories(test_function_traits PUBLIC ../include) + ament_target_dependencies(test_function_traits + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) +endif() +ament_add_gmock(test_intra_process_manager rclcpp/test_intra_process_manager.cpp) +if(TARGET test_intra_process_manager) + ament_target_dependencies(test_intra_process_manager + "rcl" + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) +endif() +ament_add_gtest(test_ring_buffer_implementation rclcpp/test_ring_buffer_implementation.cpp) +if(TARGET test_ring_buffer_implementation) + ament_target_dependencies(test_ring_buffer_implementation + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME}) +endif() +ament_add_gtest(test_intra_process_buffer rclcpp/test_intra_process_buffer.cpp) +if(TARGET test_intra_process_buffer) + ament_target_dependencies(test_intra_process_buffer + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_intra_process_buffer ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_loaned_message rclcpp/test_loaned_message.cpp) +ament_target_dependencies(test_loaned_message + "test_msgs" +) +target_link_libraries(test_loaned_message ${PROJECT_NAME}) + +ament_add_gtest(test_node rclcpp/test_node.cpp TIMEOUT 240) +if(TARGET test_node) + ament_target_dependencies(test_node + "rcl_interfaces" + "rcpputils" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_node ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_node_interfaces__get_node_interfaces + rclcpp/node_interfaces/test_get_node_interfaces.cpp) +if(TARGET test_node_interfaces__get_node_interfaces) + target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME}) +endif() + +# TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node +# rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node +# ${PROJECT_NAME}) + +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node +# rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node +# ${PROJECT_NAME}) + +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node +# rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node +# ${PROJECT_NAME}) + +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node +# rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node +# ${PROJECT_NAME}) + +ament_add_gtest(test_node_global_args rclcpp/test_node_global_args.cpp) +if(TARGET test_node_global_args) + ament_target_dependencies(test_node_global_args + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_node_global_args ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_options rclcpp/test_node_options.cpp) +if(TARGET test_node_options) + ament_target_dependencies(test_node_options "rcl") + target_link_libraries(test_node_options ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter_client rclcpp/test_parameter_client.cpp) +if(TARGET test_parameter_client) + ament_target_dependencies(test_parameter_client + "rcl_interfaces" + ) + target_link_libraries(test_parameter_client ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter_events_filter rclcpp/test_parameter_events_filter.cpp) +if(TARGET test_parameter_events_filter) + ament_target_dependencies(test_parameter_events_filter + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_parameter_events_filter ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter rclcpp/test_parameter.cpp) +if(TARGET test_parameter) + ament_target_dependencies(test_parameter + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_parameter ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter_map rclcpp/test_parameter_map.cpp) +if(TARGET test_parameter_map) + target_link_libraries(test_parameter_map ${PROJECT_NAME}) +endif() +ament_add_gtest(test_publisher rclcpp/test_publisher.cpp) +if(TARGET test_publisher) + ament_target_dependencies(test_publisher + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_publisher ${PROJECT_NAME}) +endif() +ament_add_gtest(test_publisher_subscription_count_api rclcpp/test_publisher_subscription_count_api.cpp) +if(TARGET test_publisher_subscription_count_api) + ament_target_dependencies(test_publisher_subscription_count_api + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME}) +endif() +ament_add_gtest(test_qos rclcpp/test_qos.cpp) +if(TARGET test_qos) + ament_target_dependencies(test_qos + "rmw" + ) + target_link_libraries(test_qos + ${PROJECT_NAME} + ) +endif() +ament_add_gtest(test_qos_event rclcpp/test_qos_event.cpp) +if(TARGET test_qos_event) + ament_target_dependencies(test_qos_event + "rmw" + "test_msgs" + ) + target_link_libraries(test_qos_event + ${PROJECT_NAME} + ) +endif() +ament_add_gtest(test_rate rclcpp/test_rate.cpp) +if(TARGET test_rate) + ament_target_dependencies(test_rate + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_rate + ${PROJECT_NAME} + ) +endif() +ament_add_gtest(test_serialized_message_allocator rclcpp/test_serialized_message_allocator.cpp) +if(TARGET test_serialized_message_allocator) + ament_target_dependencies(test_serialized_message_allocator + test_msgs + ) + target_link_libraries(test_serialized_message_allocator + ${PROJECT_NAME} + ) +endif() +ament_add_gtest(test_serialized_message rclcpp/test_serialized_message.cpp) +if(TARGET test_serialized_message) + ament_target_dependencies(test_serialized_message + test_msgs + ) + target_link_libraries(test_serialized_message + ${PROJECT_NAME} + ) +endif() +ament_add_gtest(test_service rclcpp/test_service.cpp) +if(TARGET test_service) + ament_target_dependencies(test_service + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_service ${PROJECT_NAME}) +endif() +ament_add_gtest(test_subscription rclcpp/test_subscription.cpp) +if(TARGET test_subscription) + ament_target_dependencies(test_subscription + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_subscription ${PROJECT_NAME}) +endif() +ament_add_gtest(test_subscription_publisher_count_api rclcpp/test_subscription_publisher_count_api.cpp) +if(TARGET test_subscription_publisher_count_api) + ament_target_dependencies(test_subscription_publisher_count_api + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME}) +endif() +ament_add_gtest(test_subscription_traits rclcpp/test_subscription_traits.cpp) +if(TARGET test_subscription_traits) + ament_target_dependencies(test_subscription_traits + "rcl" + "test_msgs" + ) + target_link_libraries(test_subscription_traits ${PROJECT_NAME}) +endif() +ament_add_gtest(test_find_weak_nodes rclcpp/test_find_weak_nodes.cpp) +if(TARGET test_find_weak_nodes) + ament_target_dependencies(test_find_weak_nodes + "rcl" + ) + target_link_libraries(test_find_weak_nodes ${PROJECT_NAME}) +endif() + +set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") +if(WIN32) + set(append_library_dirs "${append_library_dirs}/$") +endif() + +ament_add_gtest(test_externally_defined_services rclcpp/test_externally_defined_services.cpp) +ament_target_dependencies(test_externally_defined_services + "rcl" + "test_msgs" +) +target_link_libraries(test_externally_defined_services ${PROJECT_NAME}) + +ament_add_gtest(test_duration rclcpp/test_duration.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_duration) + ament_target_dependencies(test_duration + "rcl") + target_link_libraries(test_duration ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_executor rclcpp/test_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_executor) + ament_target_dependencies(test_executor + "rcl") + target_link_libraries(test_executor ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_logger rclcpp/test_logger.cpp) +target_link_libraries(test_logger ${PROJECT_NAME}) + +ament_add_gmock(test_logging rclcpp/test_logging.cpp) +target_link_libraries(test_logging ${PROJECT_NAME}) + +ament_add_gtest(test_time rclcpp/test_time.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_time) + ament_target_dependencies(test_time + "rcl") + target_link_libraries(test_time ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_timer rclcpp/test_timer.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_timer) + ament_target_dependencies(test_timer + "rcl") + target_link_libraries(test_timer ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_time_source rclcpp/test_time_source.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_time_source) + ament_target_dependencies(test_time_source + "rcl") + target_link_libraries(test_time_source ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_utilities rclcpp/test_utilities.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_utilities) + ament_target_dependencies(test_utilities + "rcl") + target_link_libraries(test_utilities ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_init rclcpp/test_init.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_init) + ament_target_dependencies(test_init + "rcl") + target_link_libraries(test_init ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_interface_traits rclcpp/test_interface_traits.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_interface_traits) + ament_target_dependencies(test_interface_traits + "rcl") + target_link_libraries(test_interface_traits ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_multi_threaded_executor rclcpp/executors/test_multi_threaded_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_multi_threaded_executor) + ament_target_dependencies(test_multi_threaded_executor + "rcl") + target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_guard_condition rclcpp/test_guard_condition.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_guard_condition) + target_link_libraries(test_guard_condition ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_wait_set rclcpp/test_wait_set.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_wait_set) + ament_target_dependencies(test_wait_set "test_msgs") + target_link_libraries(test_wait_set ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_subscription_topic_statistics rclcpp/topic_statistics/test_subscription_topic_statistics.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_subscription_topic_statistics) + ament_target_dependencies(test_subscription_topic_statistics + "builtin_interfaces" + "libstatistics_collector" + "rcl_interfaces" + "rcutils" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "statistics_msgs" + "test_msgs") + rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") + target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_subscription_options rclcpp/test_subscription_options.cpp) +if(TARGET test_subscription_options) + ament_target_dependencies(test_subscription_options "rcl") + target_link_libraries(test_subscription_options ${PROJECT_NAME}) +endif() + +# Install test resources +install( + DIRECTORY resources + DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/rclcpp/cmake/rclcpp_add_build_failure_test.cmake b/rclcpp/test/cmake/rclcpp_add_build_failure_test.cmake similarity index 100% rename from rclcpp/cmake/rclcpp_add_build_failure_test.cmake rename to rclcpp/test/cmake/rclcpp_add_build_failure_test.cmake diff --git a/rclcpp/test/executors/test_multi_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp similarity index 100% rename from rclcpp/test/executors/test_multi_threaded_executor.cpp rename to rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp diff --git a/rclcpp/test/node_interfaces/node_wrapper.hpp b/rclcpp/test/rclcpp/node_interfaces/node_wrapper.hpp similarity index 93% rename from rclcpp/test/node_interfaces/node_wrapper.hpp rename to rclcpp/test/rclcpp/node_interfaces/node_wrapper.hpp index f4319f545d..b1413e1f05 100644 --- a/rclcpp/test/node_interfaces/node_wrapper.hpp +++ b/rclcpp/test/rclcpp/node_interfaces/node_wrapper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NODE_INTERFACES__NODE_WRAPPER_HPP_ -#define NODE_INTERFACES__NODE_WRAPPER_HPP_ +#ifndef RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_ #include #include @@ -61,4 +61,4 @@ class NodeWrapper rclcpp::Node::SharedPtr node; }; -#endif // NODE_INTERFACES__NODE_WRAPPER_HPP_ +#endif // RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_ diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp similarity index 100% rename from rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp rename to rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp similarity index 100% rename from rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp rename to rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp similarity index 100% rename from rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp rename to rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp similarity index 100% rename from rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp rename to rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp diff --git a/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp b/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp similarity index 100% rename from rclcpp/test/node_interfaces/test_get_node_interfaces.cpp rename to rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp diff --git a/rclcpp/test/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp similarity index 100% rename from rclcpp/test/test_client.cpp rename to rclcpp/test/rclcpp/test_client.cpp diff --git a/rclcpp/test/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp similarity index 100% rename from rclcpp/test/test_create_timer.cpp rename to rclcpp/test/rclcpp/test_create_timer.cpp diff --git a/rclcpp/test/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp similarity index 100% rename from rclcpp/test/test_duration.cpp rename to rclcpp/test/rclcpp/test_duration.cpp diff --git a/rclcpp/test/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp similarity index 100% rename from rclcpp/test/test_executor.cpp rename to rclcpp/test/rclcpp/test_executor.cpp diff --git a/rclcpp/test/test_expand_topic_or_service_name.cpp b/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp similarity index 100% rename from rclcpp/test/test_expand_topic_or_service_name.cpp rename to rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/rclcpp/test_externally_defined_services.cpp similarity index 100% rename from rclcpp/test/test_externally_defined_services.cpp rename to rclcpp/test/rclcpp/test_externally_defined_services.cpp diff --git a/rclcpp/test/test_find_weak_nodes.cpp b/rclcpp/test/rclcpp/test_find_weak_nodes.cpp similarity index 100% rename from rclcpp/test/test_find_weak_nodes.cpp rename to rclcpp/test/rclcpp/test_find_weak_nodes.cpp diff --git a/rclcpp/test/test_function_traits.cpp b/rclcpp/test/rclcpp/test_function_traits.cpp similarity index 100% rename from rclcpp/test/test_function_traits.cpp rename to rclcpp/test/rclcpp/test_function_traits.cpp diff --git a/rclcpp/test/test_guard_condition.cpp b/rclcpp/test/rclcpp/test_guard_condition.cpp similarity index 100% rename from rclcpp/test/test_guard_condition.cpp rename to rclcpp/test/rclcpp/test_guard_condition.cpp diff --git a/rclcpp/test/test_init.cpp b/rclcpp/test/rclcpp/test_init.cpp similarity index 100% rename from rclcpp/test/test_init.cpp rename to rclcpp/test/rclcpp/test_init.cpp diff --git a/rclcpp/test/test_interface_traits.cpp b/rclcpp/test/rclcpp/test_interface_traits.cpp similarity index 100% rename from rclcpp/test/test_interface_traits.cpp rename to rclcpp/test/rclcpp/test_interface_traits.cpp diff --git a/rclcpp/test/test_intra_process_buffer.cpp b/rclcpp/test/rclcpp/test_intra_process_buffer.cpp similarity index 100% rename from rclcpp/test/test_intra_process_buffer.cpp rename to rclcpp/test/rclcpp/test_intra_process_buffer.cpp diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp similarity index 100% rename from rclcpp/test/test_intra_process_manager.cpp rename to rclcpp/test/rclcpp/test_intra_process_manager.cpp diff --git a/rclcpp/test/test_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp similarity index 100% rename from rclcpp/test/test_loaned_message.cpp rename to rclcpp/test/rclcpp/test_loaned_message.cpp diff --git a/rclcpp/test/test_logger.cpp b/rclcpp/test/rclcpp/test_logger.cpp similarity index 100% rename from rclcpp/test/test_logger.cpp rename to rclcpp/test/rclcpp/test_logger.cpp diff --git a/rclcpp/test/test_logging.cpp b/rclcpp/test/rclcpp/test_logging.cpp similarity index 100% rename from rclcpp/test/test_logging.cpp rename to rclcpp/test/rclcpp/test_logging.cpp diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp similarity index 100% rename from rclcpp/test/test_node.cpp rename to rclcpp/test/rclcpp/test_node.cpp diff --git a/rclcpp/test/test_node_global_args.cpp b/rclcpp/test/rclcpp/test_node_global_args.cpp similarity index 100% rename from rclcpp/test/test_node_global_args.cpp rename to rclcpp/test/rclcpp/test_node_global_args.cpp diff --git a/rclcpp/test/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp similarity index 100% rename from rclcpp/test/test_node_options.cpp rename to rclcpp/test/rclcpp/test_node_options.cpp diff --git a/rclcpp/test/test_parameter.cpp b/rclcpp/test/rclcpp/test_parameter.cpp similarity index 100% rename from rclcpp/test/test_parameter.cpp rename to rclcpp/test/rclcpp/test_parameter.cpp diff --git a/rclcpp/test/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp similarity index 100% rename from rclcpp/test/test_parameter_client.cpp rename to rclcpp/test/rclcpp/test_parameter_client.cpp diff --git a/rclcpp/test/test_parameter_events_filter.cpp b/rclcpp/test/rclcpp/test_parameter_events_filter.cpp similarity index 100% rename from rclcpp/test/test_parameter_events_filter.cpp rename to rclcpp/test/rclcpp/test_parameter_events_filter.cpp diff --git a/rclcpp/test/test_parameter_map.cpp b/rclcpp/test/rclcpp/test_parameter_map.cpp similarity index 100% rename from rclcpp/test/test_parameter_map.cpp rename to rclcpp/test/rclcpp/test_parameter_map.cpp diff --git a/rclcpp/test/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp similarity index 100% rename from rclcpp/test/test_publisher.cpp rename to rclcpp/test/rclcpp/test_publisher.cpp diff --git a/rclcpp/test/test_publisher_subscription_count_api.cpp b/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp similarity index 100% rename from rclcpp/test/test_publisher_subscription_count_api.cpp rename to rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp diff --git a/rclcpp/test/test_qos.cpp b/rclcpp/test/rclcpp/test_qos.cpp similarity index 100% rename from rclcpp/test/test_qos.cpp rename to rclcpp/test/rclcpp/test_qos.cpp diff --git a/rclcpp/test/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp similarity index 100% rename from rclcpp/test/test_qos_event.cpp rename to rclcpp/test/rclcpp/test_qos_event.cpp diff --git a/rclcpp/test/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp similarity index 100% rename from rclcpp/test/test_rate.cpp rename to rclcpp/test/rclcpp/test_rate.cpp diff --git a/rclcpp/test/test_ring_buffer_implementation.cpp b/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp similarity index 100% rename from rclcpp/test/test_ring_buffer_implementation.cpp rename to rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp diff --git a/rclcpp/test/test_serialized_message.cpp b/rclcpp/test/rclcpp/test_serialized_message.cpp similarity index 100% rename from rclcpp/test/test_serialized_message.cpp rename to rclcpp/test/rclcpp/test_serialized_message.cpp diff --git a/rclcpp/test/test_serialized_message_allocator.cpp b/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp similarity index 100% rename from rclcpp/test/test_serialized_message_allocator.cpp rename to rclcpp/test/rclcpp/test_serialized_message_allocator.cpp diff --git a/rclcpp/test/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp similarity index 100% rename from rclcpp/test/test_service.cpp rename to rclcpp/test/rclcpp/test_service.cpp diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp similarity index 100% rename from rclcpp/test/test_subscription.cpp rename to rclcpp/test/rclcpp/test_subscription.cpp diff --git a/rclcpp/test/test_subscription_options.cpp b/rclcpp/test/rclcpp/test_subscription_options.cpp similarity index 100% rename from rclcpp/test/test_subscription_options.cpp rename to rclcpp/test/rclcpp/test_subscription_options.cpp diff --git a/rclcpp/test/test_subscription_publisher_count_api.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp similarity index 100% rename from rclcpp/test/test_subscription_publisher_count_api.cpp rename to rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp diff --git a/rclcpp/test/test_subscription_traits.cpp b/rclcpp/test/rclcpp/test_subscription_traits.cpp similarity index 100% rename from rclcpp/test/test_subscription_traits.cpp rename to rclcpp/test/rclcpp/test_subscription_traits.cpp diff --git a/rclcpp/test/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp similarity index 100% rename from rclcpp/test/test_time.cpp rename to rclcpp/test/rclcpp/test_time.cpp diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp similarity index 100% rename from rclcpp/test/test_time_source.cpp rename to rclcpp/test/rclcpp/test_time_source.cpp diff --git a/rclcpp/test/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp similarity index 100% rename from rclcpp/test/test_timer.cpp rename to rclcpp/test/rclcpp/test_timer.cpp diff --git a/rclcpp/test/test_utilities.cpp b/rclcpp/test/rclcpp/test_utilities.cpp similarity index 100% rename from rclcpp/test/test_utilities.cpp rename to rclcpp/test/rclcpp/test_utilities.cpp diff --git a/rclcpp/test/test_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp similarity index 100% rename from rclcpp/test/test_wait_set.cpp rename to rclcpp/test/rclcpp/test_wait_set.cpp diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp similarity index 100% rename from rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp rename to rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp diff --git a/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp b/rclcpp/test/rclcpp/topic_statistics/test_topic_stats_utils.hpp similarity index 95% rename from rclcpp/test/topic_statistics/test_topic_stats_utils.hpp rename to rclcpp/test/rclcpp/topic_statistics/test_topic_stats_utils.hpp index 1399abbaff..ec50d16eaf 100644 --- a/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp +++ b/rclcpp/test/rclcpp/topic_statistics/test_topic_stats_utils.hpp @@ -22,8 +22,8 @@ #include "statistics_msgs/msg/metrics_message.hpp" -#ifndef TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ -#define TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ +#ifndef RCLCPP__TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ +#define RCLCPP__TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ namespace rclcpp { @@ -149,4 +149,4 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter } // namespace topic_statistics } // namespace rclcpp -#endif // TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ +#endif // RCLCPP__TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ From e9adc3e1840bf165d4a0cb46cfec1a5a62c6717f Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 16 Jun 2020 16:07:11 -0300 Subject: [PATCH 096/121] Add spin_all method to Executor (#1156) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/executor.hpp | 23 +++++++- rclcpp/src/rclcpp/executor.cpp | 29 ++++++++-- rclcpp/test/rclcpp/test_executor.cpp | 82 ++++++++++++++++++++++++++++ 3 files changed, 129 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 557e4f3ab3..a4c9f0b943 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -160,7 +160,7 @@ class Executor void spin_node_some(std::shared_ptr node); - /// Complete all available queued work without blocking. + /// Collect work once and execute all available work, optionally within a duration. /** * This function can be overridden. The default implementation is suitable for a * single-threaded model of execution. @@ -175,6 +175,23 @@ class Executor virtual void spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)); + /// Collect and execute work repeatedly within a duration or until no more work is available. + /** + * This function can be overridden. The default implementation is suitable for a + * single-threaded model of execution. + * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function + * to block (which may have unintended consequences). + * If the time that waitables take to be executed is longer than the period on which new waitables + * become ready, this method will execute work repeatedly until `max_duration` has elapsed. + * + * \param[in] max_duration The maximum amount of time to spend executing work. Must be positive. + * Note that spin_all() may take longer than this time as it only returns once max_duration has + * been exceeded. + */ + RCLCPP_PUBLIC + virtual void + spin_all(std::chrono::nanoseconds max_duration); + RCLCPP_PUBLIC virtual void spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); @@ -270,6 +287,10 @@ class Executor rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, std::chrono::nanoseconds timeout); + RCLCPP_PUBLIC + void + spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); + /// 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, diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 0c83e0deba..5aac2284ed 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -28,6 +28,8 @@ #include "rcutils/logging_macros.h" +using namespace std::chrono_literals; + using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::AnyExecutable; using rclcpp::Executor; @@ -212,8 +214,21 @@ 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) +{ + return this->spin_some_impl(max_duration, false); +} + +void Executor::spin_all(std::chrono::nanoseconds max_duration) +{ + if (max_duration <= 0ns) { + throw std::invalid_argument("max_duration must be positive"); + } + return this->spin_some_impl(max_duration, true); +} + void -Executor::spin_some(std::chrono::nanoseconds max_duration) +Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) { auto start = std::chrono::steady_clock::now(); auto max_duration_not_elapsed = [max_duration, start]() { @@ -232,14 +247,20 @@ Executor::spin_some(std::chrono::nanoseconds max_duration) throw std::runtime_error("spin_some() called while already spinning"); } RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); - // non-blocking call to pre-load all available work - wait_for_work(std::chrono::milliseconds::zero()); + bool work_available = false; while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { AnyExecutable any_exec; + if (!work_available) { + wait_for_work(std::chrono::milliseconds::zero()); + } if (get_next_ready_executable(any_exec)) { execute_any_executable(any_exec); + work_available = true; } else { - break; + if (!work_available || !exhaustive) { + break; + } + work_available = false; } } } diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index cdefec71e1..731add17c1 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -115,3 +115,85 @@ TEST_F(TestExecutors, testSpinUntilFutureCompleteSharedFuture) { ret = executor.spin_until_future_complete(future.share(), 100ms); EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); } + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() + { + rcl_guard_condition_options_t guard_condition_options = + rcl_guard_condition_get_default_options(); + + gc_ = rcl_get_zero_initialized_guard_condition(); + rcl_ret_t ret = rcl_guard_condition_init( + &gc_, + rclcpp::contexts::get_global_default_context()->get_rcl_context().get(), + guard_condition_options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override + { + rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); + if (RCL_RET_OK != ret) { + return false; + } + ret = rcl_trigger_guard_condition(&gc_); + return RCL_RET_OK == ret; + } + + bool + is_ready(rcl_wait_set_t * wait_set) override + { + (void)wait_set; + return true; + } + + void + execute() override + { + count_++; + std::this_thread::sleep_for(100ms); + } + + size_t + get_number_of_ready_guard_conditions() override {return 1;} + + size_t + get_count() + { + return count_; + } + +private: + size_t count_ = 0; + rcl_guard_condition_t gc_; +}; + +TEST_F(TestExecutors, testSpinAllvsSpinSome) { + { + rclcpp::executors::SingleThreadedExecutor executor; + auto waitable_interfaces = node->get_node_waitables_interface(); + auto my_waitable = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable, nullptr); + executor.add_node(node); + executor.spin_all(1s); + executor.remove_node(node); + EXPECT_GT(my_waitable->get_count(), 1u); + waitable_interfaces->remove_waitable(my_waitable, nullptr); + } + { + rclcpp::executors::SingleThreadedExecutor executor; + auto waitable_interfaces = node->get_node_waitables_interface(); + auto my_waitable = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable, nullptr); + executor.add_node(node); + executor.spin_some(1s); + executor.remove_node(node); + EXPECT_EQ(my_waitable->get_count(), 1u); + waitable_interfaces->remove_waitable(my_waitable, nullptr); + } +} From 71ef44f292fedfbdd2303dae25f96b7be03b50e9 Mon Sep 17 00:00:00 2001 From: tomoya Date: Wed, 17 Jun 2020 07:03:35 +0900 Subject: [PATCH 097/121] add rcl_action_client_options when creating action client. (#1133) * add rcl_action_client_options for create_client. Signed-off-by: Tomoya.Fujita * Capitalize comments and keep the default rcl_action_client_options. Signed-off-by: Tomoya.Fujita * delete unnecessary default rcl_action_client_options_t. Signed-off-by: Tomoya.Fujita --- rclcpp/include/rclcpp/publisher.hpp | 2 +- rclcpp/include/rclcpp/subscription.hpp | 2 +- rclcpp/include/rclcpp/subscription_base.hpp | 2 +- rclcpp_action/include/rclcpp_action/client.hpp | 2 +- .../include/rclcpp_action/create_client.hpp | 14 ++++++++++---- .../include/rclcpp_action/create_server.hpp | 4 ++-- rclcpp_action/include/rclcpp_action/server.hpp | 2 +- rclcpp_action/test/test_client.cpp | 4 +++- 8 files changed, 20 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 8518353574..d8b572c90c 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -68,7 +68,7 @@ class Publisher : public PublisherBase * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup. * \param[in] topic Name of the topic to publish to. * \param[in] qos QoS profile for Subcription. - * \param[in] options options for the subscription. + * \param[in] options Options for the subscription. */ Publisher( rclcpp::node_interfaces::NodeBaseInterface * node_base, diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index b567d1910e..679a0c8ff1 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -92,7 +92,7 @@ class Subscription : public SubscriptionBase * \param[in] topic_name Name of the topic to subscribe to. * \param[in] qos QoS profile for Subcription. * \param[in] callback User defined callback to call when a message is received. - * \param[in] options options for the subscription. + * \param[in] options Options for the subscription. * \param[in] message_memory_strategy The memory strategy to be used for managing message memory. * \param[in] subscription_topic_statistics Optional pointer to a topic statistics subcription. * \throws std::invalid_argument if the QoS is uncompatible with intra-process (if one diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 68df9211bd..28b92ffb13 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -66,7 +66,7 @@ class SubscriptionBase : public std::enable_shared_from_this * \param[in] node_base NodeBaseInterface pointer used in parts of the setup. * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. * \param[in] topic_name Name of the topic to subscribe to. - * \param[in] subscription_options options for the subscription. + * \param[in] subscription_options Options for the subscription. * \param[in] is_serialized is true if the message will be delivered still serialized */ RCLCPP_PUBLIC diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 71b0c866a0..a46e008f33 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -317,7 +317,7 @@ 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, diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 7c3f210cb6..8538a8205e 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -37,6 +37,7 @@ namespace rclcpp_action * \param[in] name The action name. * \param[in] group The action client will be added to this callback group. * If `nullptr`, then the action client is added to the default callback group. + * \param[in] options Options to pass to the underlying `rcl_action_client_t`. */ template typename Client::SharedPtr @@ -46,7 +47,8 @@ create_client( rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, const std::string & name, - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + const rcl_action_client_options_t & options = rcl_action_client_get_default_options()) { std::weak_ptr weak_node = node_waitables_interface; @@ -82,7 +84,8 @@ create_client( node_base_interface, node_graph_interface, node_logging_interface, - name), + name, + options), deleter); node_waitables_interface->add_waitable(action_client, group); @@ -95,13 +98,15 @@ create_client( * \param[in] name The action name. * \param[in] group The action client will be added to this callback group. * If `nullptr`, then the action client is added to the default callback group. + * \param[in] options Options to pass to the underlying `rcl_action_client_t`. */ template typename Client::SharedPtr create_client( NodeT node, const std::string & name, - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + const rcl_action_client_options_t & options = rcl_action_client_get_default_options()) { return rclcpp_action::create_client( node->get_node_base_interface(), @@ -109,7 +114,8 @@ create_client( node->get_node_logging_interface(), node->get_node_waitables_interface(), name, - group); + group, + options); } } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 189bc710a4..9116fc05ee 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -49,7 +49,7 @@ namespace rclcpp_action * The return from this callback only indicates if the server will try to cancel a goal. * It does not indicate if the goal was actually canceled. * \param[in] handle_accepted A callback that is called to give the user a handle to the goal. - * \param[in] options options to pass to the underlying `rcl_action_server_t`. + * \param[in] options Options to pass to the underlying `rcl_action_server_t`. * \param[in] group The action server will be added to this callback group. * If `nullptr`, then the action server is added to the default callback group. */ @@ -123,7 +123,7 @@ create_server( * The return from this callback only indicates if the server will try to cancel a goal. * It does not indicate if the goal was actually canceled. * \param[in] handle_accepted A callback that is called to give the user a handle to the goal. - * \param[in] options options to pass to the underlying `rcl_action_server_t`. + * \param[in] options Options to pass to the underlying `rcl_action_server_t`. * \param[in] group The action server will be added to this callback group. * If `nullptr`, then the action server is added to the default callback group. */ diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 7aede9ae69..c5ff2c1d8c 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -296,7 +296,7 @@ class Server : public ServerBase, public std::enable_shared_from_thiscreate_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); + const rcl_action_client_options_t & options = rcl_action_client_get_default_options(); ASSERT_NO_THROW( rclcpp_action::create_client( client_node->get_node_base_interface(), @@ -320,7 +321,8 @@ TEST_F(TestClient, construction_and_destruction_callback_group) client_node->get_node_logging_interface(), client_node->get_node_waitables_interface(), action_name, - group + group, + options ).reset()); } From a3f73dc0214779e64b0d2c7e85d26264f56f0b77 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 17 Jun 2020 18:58:39 -0300 Subject: [PATCH 098/121] Add message lost subscription event (#1164) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/qos_event.hpp | 3 +++ rclcpp/include/rclcpp/subscription.hpp | 5 +++++ 2 files changed, 8 insertions(+) diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index a43d904f10..0bce0aac37 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -34,6 +34,7 @@ using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t; using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t; using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t; using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t; +using QOSMessageLostInfo = rmw_message_lost_status_t; using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t; using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t; @@ -41,6 +42,7 @@ using QOSDeadlineRequestedCallbackType = std::function; using QOSLivelinessChangedCallbackType = std::function; using QOSLivelinessLostCallbackType = std::function; +using QOSMessageLostCallbackType = std::function; using QOSOfferedIncompatibleQoSCallbackType = std::function; using QOSRequestedIncompatibleQoSCallbackType = std::function; @@ -59,6 +61,7 @@ struct SubscriptionEventCallbacks QOSDeadlineRequestedCallbackType deadline_callback; QOSLivelinessChangedCallbackType liveliness_callback; QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback; + QOSMessageLostCallbackType message_lost_callback; }; class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 679a0c8ff1..42bcb26dc8 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -144,6 +144,11 @@ class Subscription : public SubscriptionBase // pass } } + if (options.event_callbacks.message_lost_callback) { + this->add_event_handler( + options.event_callbacks.message_lost_callback, + RCL_SUBSCRIPTION_MESSAGE_LOST); + } // Setup intra process publishing if requested. if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) { From a508887691de68fd6ed9986ddc4b7afd5afab921 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 18 Jun 2020 09:51:03 -0700 Subject: [PATCH 099/121] Fix get_node_time_source_interface() docstring (#988) Signed-off-by: Jacob Perron --- rclcpp/include/rclcpp/node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7706fa8a66..08e42bd634 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -1063,7 +1063,7 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface(); - /// Return the Node's internal NodeParametersInterface implementation. + /// Return the Node's internal NodeTimeSourceInterface implementation. RCLCPP_PUBLIC rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface(); From 4f4b95d0a7dc1b9820b48a08533ca6a9596d9980 Mon Sep 17 00:00:00 2001 From: brawner Date: Thu, 18 Jun 2020 14:00:31 -0700 Subject: [PATCH 100/121] Check period duration in create_wall_timer (#1178) * Check period duration in create_wall_timer Signed-off-by: Stephen Brawner * Adding comments Signed-off-by: Stephen Brawner --- rclcpp/include/rclcpp/create_timer.hpp | 38 +++++++++++++--- rclcpp/test/rclcpp/test_create_timer.cpp | 57 +++++++++++++++++++++++- 2 files changed, 89 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index f7b810dc4d..345f43fcbb 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -76,14 +76,14 @@ create_timer( * \tparam DurationRepT * \tparam DurationT * \tparam CallbackT - * \param period period to exectute callback + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() * \param callback callback to execute via the timer period * \param group * \param node_base * \param node_timers * \return * \throws std::invalid argument if either node_base or node_timers - * are null + * are null, or period is negative or too large */ template typename rclcpp::WallTimer::SharedPtr @@ -102,10 +102,38 @@ create_wall_timer( throw std::invalid_argument{"input node_timers cannot be null"}; } + if (period < std::chrono::duration::zero()) { + throw std::invalid_argument{"timer period cannot be negative"}; + } + + // Casting to a double representation might lose precision and allow the check below to succeed + // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. + constexpr auto maximum_safe_cast_ns = + std::chrono::nanoseconds::max() - std::chrono::duration(1); + + // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow + // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is + // greater than nanoseconds::max() is a difficult general problem. This is a more conservative + // version of Howard Hinnant's (the guy>) response here: + // https://stackoverflow.com/a/44637334/2089061 + // However, this doesn't solve the issue for all possible duration types of period. + // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 + constexpr auto ns_max_as_double = + std::chrono::duration_cast>( + maximum_safe_cast_ns); + if (period > ns_max_as_double) { + throw std::invalid_argument{ + "timer period must be less than std::chrono::nanoseconds::max()"}; + } + + const auto period_ns = std::chrono::duration_cast(period); + if (period_ns < std::chrono::nanoseconds::zero()) { + throw std::runtime_error{ + "Casting timer period to nanoseconds resulted in integer overflow."}; + } + auto timer = rclcpp::WallTimer::make_shared( - std::chrono::duration_cast(period), - std::move(callback), - node_base->get_context()); + period_ns, std::move(callback), node_base->get_context()); node_timers->add_timer(timer, group); return timer; } diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index cff870a8a4..f6acd79b24 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/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; @@ -61,3 +61,58 @@ TEST(TestCreateTimer, call_with_node_wrapper_compiles) []() {}); rclcpp::shutdown(); } + +TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_wall_timers_with_bad_arguments"); + auto callback = []() {}; + rclcpp::CallbackGroup::SharedPtr group = nullptr; + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + + // Negative period + EXPECT_THROW( + rclcpp::create_wall_timer(-1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Very negative period + constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_wall_timer( + nanoseconds_min, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Period must be less than nanoseconds::max() + constexpr auto nanoseconds_max = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_wall_timer( + nanoseconds_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + EXPECT_NO_THROW( + rclcpp::create_wall_timer( + nanoseconds_max - 1us, callback, group, node_interface, timers_interface)); + + EXPECT_NO_THROW( + rclcpp::create_wall_timer(0ms, callback, group, node_interface, timers_interface)); + + // Period must be less than nanoseconds::max() + constexpr auto hours_max = std::chrono::hours::max(); + EXPECT_THROW( + rclcpp::create_wall_timer(hours_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_wall_timer(1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_wall_timer(1ms, callback, group, node_interface, nullptr), + std::invalid_argument); + rclcpp::shutdown(); +} From b8cb2fcc3ca65812c89778d0d6c064aedd99d62d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 18 Jun 2020 21:10:46 +0000 Subject: [PATCH 101/121] Changelogs Signed-off-by: Ivan Santiago Paunovic --- rclcpp/CHANGELOG.rst | 15 +++++++++++++++ rclcpp_action/CHANGELOG.rst | 7 +++++++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 30 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 6212be7884..b5a412eb0b 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Check period duration in create_wall_timer (`#1178 `_) +* Fix get_node_time_source_interface() docstring (`#988 `_) +* Add message lost subscription event (`#1164 `_) +* Add spin_all method to Executor (`#1156 `_) +* Reorganize test directory and split CMakeLists.txt (`#1173 `_) +* Check if context is valid when looping in spin_some (`#1167 `_) +* Add check for invalid topic statistics publish period (`#1151 `_) +* Fix spin_until_future_complete: check spinning value (`#1023 `_) +* Fix doxygen warnings (`#1163 `_) +* Fix reference to rclcpp in its Quality declaration (`#1161 `_) +* Allow spin_until_future_complete to accept any future like object (`#1113 `_) +* Contributors: Alejandro Hernández Cordero, Christophe Bedard, Devin Bonnie, Dirk Thomas, DongheeYe, Ivan Santiago Paunovic, Jacob Perron, Sarthak Mittal, brawner, tomoya + 2.0.0 (2020-06-01) ------------------ * Added missing virtual destructors. (`#1149 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 2e59cd044f..d2526e7882 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add rcl_action_client_options when creating action client. (`#1133 `_) +* Fix doxygen warnings (`#1163 `_) +* Increase rclcpp_action test coverage (`#1153 `_) +* Contributors: Alejandro Hernández Cordero, Michel Hidalgo, tomoya + 2.0.0 (2020-06-01) ------------------ * Added missing virtual destructors. (`#1149 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index b096270a3a..1fe87ea065 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2020-06-01) ------------------ * Added missing virtual destructors. (`#1149 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index ea6404470d..3e0d5f0de9 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix doxygen warnings (`#1163 `_) +* Contributors: Alejandro Hernández Cordero + 2.0.0 (2020-06-01) ------------------ * Added missing virtual destructors. (`#1149 `_) From 456ffe735fa495c08b8ca33a770e5bc0cee70920 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 18 Jun 2020 21:11:09 +0000 Subject: [PATCH 102/121] 3.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index b5a412eb0b..5fe07dd514 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2020-06-18) +------------------ * Check period duration in create_wall_timer (`#1178 `_) * Fix get_node_time_source_interface() docstring (`#988 `_) * Add message lost subscription event (`#1164 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index edbcc7fcdc..d7d3c8b71d 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 2.0.0 + 3.0.0 The ROS client library in C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index d2526e7882..1e4e047169 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2020-06-18) +------------------ * Add rcl_action_client_options when creating action client. (`#1133 `_) * Fix doxygen warnings (`#1163 `_) * Increase rclcpp_action test coverage (`#1153 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 4dfa1764f2..1409f136ad 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 2.0.0 + 3.0.0 Adds action APIs for C++. Dirk Thomas Apache License 2.0 diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 1fe87ea065..a962514bb7 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2020-06-18) +------------------ 2.0.0 (2020-06-01) ------------------ diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 8d004a7f58..a0f025be1e 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 2.0.0 + 3.0.0 Package containing tools for dynamically loadable components Michael Carroll Apache License 2.0 diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 3e0d5f0de9..28fcd7e411 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2020-06-18) +------------------ * Fix doxygen warnings (`#1163 `_) * Contributors: Alejandro Hernández Cordero diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 10e9a6f657..32b426aaf9 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 2.0.0 + 3.0.0 Package containing a prototype for lifecycle implementation Karsten Knese Apache License 2.0 From 7b53a2eeff8da147b872aea4730c7e5f21750350 Mon Sep 17 00:00:00 2001 From: brawner Date: Fri, 19 Jun 2020 10:57:06 -0700 Subject: [PATCH 103/121] Add create_publisher include to create_subscription (#1180) Signed-off-by: Stephen Brawner --- rclcpp/include/rclcpp/create_subscription.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 2eb24b48b5..438cb72e72 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -29,6 +29,7 @@ #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/create_publisher.hpp" #include "rclcpp/create_timer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/subscription_factory.hpp" From 98999c3b65ce0af8ec2c64f48e5e2f6fef8067a4 Mon Sep 17 00:00:00 2001 From: brawner Date: Fri, 19 Jun 2020 13:22:06 -0700 Subject: [PATCH 104/121] Add unit tests for logging functionality (#1184) Signed-off-by: Stephen Brawner --- rclcpp/test/rclcpp/test_logger.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/rclcpp/test/rclcpp/test_logger.cpp b/rclcpp/test/rclcpp/test_logger.cpp index 258cdd0959..3eba40f20e 100644 --- a/rclcpp/test/rclcpp/test_logger.cpp +++ b/rclcpp/test/rclcpp/test_logger.cpp @@ -14,10 +14,12 @@ #include +#include #include #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" +#include "rclcpp/node.hpp" TEST(TestLogger, factory_functions) { rclcpp::Logger logger = rclcpp::get_logger("test_logger"); @@ -33,3 +35,15 @@ TEST(TestLogger, hierarchy) { rclcpp::Logger subsublogger = sublogger.get_child("grandchild"); EXPECT_STREQ("test_logger.child.grandchild", subsublogger.get_name()); } + +TEST(TestLogger, get_node_logger) { + rclcpp::init(0, nullptr); + auto node = std::make_shared("my_node", "/ns"); + auto node_base = rclcpp::node_interfaces::get_node_base_interface(node); + auto logger = rclcpp::get_node_logger(node_base->get_rcl_node_handle()); + EXPECT_STREQ(logger.get_name(), "ns.my_node"); + + logger = rclcpp::get_node_logger(nullptr); + EXPECT_STREQ(logger.get_name(), "rclcpp"); + rclcpp::shutdown(); +} From 2373ddf18dd19d9b6caa8fbe0a3b5c74c40898d0 Mon Sep 17 00:00:00 2001 From: tomoya Date: Tue, 23 Jun 2020 03:25:59 +0900 Subject: [PATCH 105/121] callback should be perfectly-forwarded. (#1183) Signed-off-by: Tomoya.Fujita --- rclcpp/include/rclcpp/client.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 41b49245bb..bd2d326012 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -361,7 +361,8 @@ class Client : public ClientBase SharedPromiseWithRequest promise = std::make_shared(); SharedFutureWithRequest future_with_request(promise->get_future()); - auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) { + auto wrapping_cb = [future_with_request, promise, request, + cb = std::forward(cb)](SharedFuture future) { auto response = future.get(); promise->set_value(std::make_pair(request, response)); cb(future_with_request); From 88f15c71718e9569c124f2fa4b667e7387791a2a Mon Sep 17 00:00:00 2001 From: brawner Date: Mon, 22 Jun 2020 13:37:54 -0700 Subject: [PATCH 106/121] Unit tests for some header-only functions/classes (#1181) * Unit tests for header-only functions/classes Adds coverage for: * any_service_callback.hpp * any_subscription_callback.hpp * create_subscription.hpp * create_timer.hpp Signed-off-by: Stephen Brawner * Address PR feedback Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 21 ++ .../test/rclcpp/test_any_service_callback.cpp | 78 +++++++ .../rclcpp/test_any_subscription_callback.cpp | 205 ++++++++++++++++++ .../test/rclcpp/test_create_subscription.cpp | 67 ++++++ 4 files changed, 371 insertions(+) create mode 100644 rclcpp/test/rclcpp/test_any_service_callback.cpp create mode 100644 rclcpp/test/rclcpp/test_any_subscription_callback.cpp create mode 100644 rclcpp/test/rclcpp/test_create_subscription.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index d500f3e58a..46fe7c15d6 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -17,6 +17,20 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs SKIP_INSTALL ) +ament_add_gtest(test_any_service_callback rclcpp/test_any_service_callback.cpp) +if(TARGET test_any_service_callback) + ament_target_dependencies(test_any_service_callback + "test_msgs" + ) + target_link_libraries(test_any_service_callback ${PROJECT_NAME}) +endif() +ament_add_gtest(test_any_subscription_callback rclcpp/test_any_subscription_callback.cpp) +if(TARGET test_any_subscription_callback) + ament_target_dependencies(test_any_subscription_callback + "test_msgs" + ) + target_link_libraries(test_any_subscription_callback ${PROJECT_NAME}) +endif() ament_add_gtest(test_client rclcpp/test_client.cpp) if(TARGET test_client) ament_target_dependencies(test_client @@ -39,6 +53,13 @@ if(TARGET test_create_timer) target_link_libraries(test_create_timer ${PROJECT_NAME}) target_include_directories(test_create_timer PRIVATE rclcpp/) endif() +ament_add_gtest(test_create_subscription rclcpp/test_create_subscription.cpp) +if(TARGET test_create_subscription) + target_link_libraries(test_create_subscription ${PROJECT_NAME}) + ament_target_dependencies(test_create_subscription + "test_msgs" + ) +endif() ament_add_gtest(test_expand_topic_or_service_name rclcpp/test_expand_topic_or_service_name.cpp) if(TARGET test_expand_topic_or_service_name) ament_target_dependencies(test_expand_topic_or_service_name diff --git a/rclcpp/test/rclcpp/test_any_service_callback.cpp b/rclcpp/test/rclcpp/test_any_service_callback.cpp new file mode 100644 index 0000000000..d16c11921e --- /dev/null +++ b/rclcpp/test/rclcpp/test_any_service_callback.cpp @@ -0,0 +1,78 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This file includes basic API tests for the AnyServiceCallback class. +// It is also tested in test_externally_defined_services.cpp + +#include + +#include +#include +#include + +#include "rclcpp/any_service_callback.hpp" +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/empty.h" + +class TestAnyServiceCallback : public ::testing::Test +{ +public: + void SetUp() + { + request_header_ = std::make_shared(); + request_ = std::make_shared(); + response_ = std::make_shared(); + } + +protected: + rclcpp::AnyServiceCallback any_service_callback_; + std::shared_ptr request_header_; + std::shared_ptr request_; + std::shared_ptr response_; +}; + +TEST_F(TestAnyServiceCallback, no_set_and_dispatch_throw) { + EXPECT_THROW( + any_service_callback_.dispatch(request_header_, request_, response_), + std::runtime_error); +} + +TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) { + int callback_calls = 0; + auto callback = [&callback_calls](const std::shared_ptr, + std::shared_ptr) { + callback_calls++; + }; + + any_service_callback_.set(callback); + EXPECT_NO_THROW( + any_service_callback_.dispatch(request_header_, request_, response_)); + EXPECT_EQ(callback_calls, 1); +} + + +TEST_F(TestAnyServiceCallback, set_and_dispatch_header) { + int callback_with_header_calls = 0; + auto callback_with_header = + [&callback_with_header_calls](const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) { + callback_with_header_calls++; + }; + + any_service_callback_.set(callback_with_header); + EXPECT_NO_THROW( + any_service_callback_.dispatch(request_header_, request_, response_)); + EXPECT_EQ(callback_with_header_calls, 1); +} diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp new file mode 100644 index 0000000000..81316818fc --- /dev/null +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -0,0 +1,205 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rclcpp/any_subscription_callback.hpp" +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/msg/empty.h" + +class TestAnySubscriptionCallback : public ::testing::Test +{ +public: + TestAnySubscriptionCallback() + : any_subscription_callback_(allocator_) {} + void SetUp() + { + msg_shared_ptr_ = std::make_shared(); + msg_const_shared_ptr_ = std::make_shared(); + msg_unique_ptr_ = std::make_unique(); + } + +protected: + std::shared_ptr> allocator_; + rclcpp::AnySubscriptionCallback> + any_subscription_callback_; + + std::shared_ptr msg_shared_ptr_; + std::shared_ptr msg_const_shared_ptr_; + std::unique_ptr msg_unique_ptr_; + rclcpp::MessageInfo message_info_; +}; + +TEST_F(TestAnySubscriptionCallback, construct_destruct) { +} + +TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { + EXPECT_THROW( + any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), + std::runtime_error); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_shared_ptr) { + int callback_count = 0; + auto shared_ptr_callback = [&callback_count]( + const std::shared_ptr) { + callback_count++; + }; + + any_subscription_callback_.set(shared_ptr_callback); + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + // Can't convert ConstSharedPtr to SharedPtr + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 1); + + // Promotes Unique into SharedPtr + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); + EXPECT_EQ(callback_count, 2); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_shared_ptr_w_info) { + int callback_count = 0; + auto shared_ptr_w_info_callback = [&callback_count]( + const std::shared_ptr, const rclcpp::MessageInfo &) { + callback_count++; + }; + + any_subscription_callback_.set(shared_ptr_w_info_callback); + + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + // Can't convert ConstSharedPtr to SharedPtr + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 1); + + // Promotes Unique into SharedPtr + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); + EXPECT_EQ(callback_count, 2); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_const_shared_ptr) { + int callback_count = 0; + auto const_shared_ptr_callback = [&callback_count]( + std::shared_ptr) { + callback_count++; + }; + + any_subscription_callback_.set(const_shared_ptr_callback); + + // Ok to promote shared_ptr to ConstSharedPtr + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 2); + + // Not allowed to convert unique_ptr to const shared_ptr + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 2); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_const_shared_ptr_w_info) { + int callback_count = 0; + auto const_shared_ptr_callback = [&callback_count]( + std::shared_ptr, const rclcpp::MessageInfo &) { + callback_count++; + }; + + any_subscription_callback_.set( + std::move(const_shared_ptr_callback)); + + // Ok to promote shared_ptr to ConstSharedPtr + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 2); + + // Not allowed to convert unique_ptr to const shared_ptr + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 2); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_unique_ptr) { + int callback_count = 0; + auto unique_ptr_callback = [&callback_count]( + std::unique_ptr) { + callback_count++; + }; + + any_subscription_callback_.set(unique_ptr_callback); + + // Message is copied into unique_ptr + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 1); + + // Unique_ptr is_moved + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); + EXPECT_EQ(callback_count, 2); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_unique_ptr_w_info) { + int callback_count = 0; + auto unique_ptr_callback = [&callback_count]( + std::unique_ptr, const rclcpp::MessageInfo &) { + callback_count++; + }; + + any_subscription_callback_.set(unique_ptr_callback); + + // Message is copied into unique_ptr + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 1); + + // Unique_ptr is_moved + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); + EXPECT_EQ(callback_count, 2); +} diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp new file mode 100644 index 0000000000..184904b8e7 --- /dev/null +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -0,0 +1,67 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/create_subscription.hpp" +#include "rclcpp/node.hpp" +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/msg/empty.h" + +using namespace std::chrono_literals; + +class TestCreateSubscription : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + } + + void TearDown() override + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestCreateSubscription, create) { + auto node = std::make_shared("my_node", "/ns"); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription = + rclcpp::create_subscription(node, "topic_name", qos, callback, options); + + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); +} + +TEST_F(TestCreateSubscription, create_with_statistics) { + auto node = std::make_shared("my_node", "/ns"); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; + options.topic_stats_options.publish_topic = "topic_statistics"; + options.topic_stats_options.publish_period = 5min; + + auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription = + rclcpp::create_subscription(node, "topic_name", qos, callback, options); + + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); +} From ec8e0d07bade6bc2d39d50bc75a773bd37eb1525 Mon Sep 17 00:00:00 2001 From: brawner Date: Mon, 22 Jun 2020 14:25:51 -0700 Subject: [PATCH 107/121] Throw exception if rcl_timer_init fails (#1179) * Throw exception if rcl_timer_init fails Signed-off-by: Stephen Brawner * Add bad-argument tests for GenericTimer Signed-off-by: Stephen Brawner * Add comments Signed-off-by: Stephen Brawner * Address feedback Signed-off-by: Stephen Brawner * Address feedback Signed-off-by: Stephen Brawner --- rclcpp/src/rclcpp/timer.cpp | 14 ++++------- rclcpp/test/rclcpp/test_timer.cpp | 40 +++++++++++++++++++++++++++++++ 2 files changed, 45 insertions(+), 9 deletions(-) diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 9ece178c28..50df28baaa 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -61,15 +61,11 @@ TimerBase::TimerBase( rcl_clock_t * clock_handle = clock_->get_clock_handle(); { std::lock_guard clock_guard(clock_->get_clock_mutex()); - if ( - rcl_timer_init( - 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); - rcl_reset_error(); + rcl_ret_t ret = rcl_timer_init( + timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr, + rcl_get_default_allocator()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize rcl timer handle"); } } } diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 2d9022e045..7f85a1c4c5 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rcl/timer.h" @@ -151,3 +152,42 @@ TEST_F(TestTimer, test_run_cancel_timer) EXPECT_TRUE(has_timer_run.load()); EXPECT_TRUE(timer->is_canceled()); } + +TEST_F(TestTimer, test_bad_arguments) { + auto node_base = rclcpp::node_interfaces::get_node_base_interface(test_node); + auto context = node_base->get_context(); + + auto steady_clock = std::make_shared(RCL_STEADY_TIME); + + // Negative period + EXPECT_THROW( + rclcpp::GenericTimer(steady_clock, -1ms, []() {}, context), + rclcpp::exceptions::RCLInvalidArgument); + + // Very negative period + constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::GenericTimer( + steady_clock, nanoseconds_min, []() {}, context), + rclcpp::exceptions::RCLInvalidArgument); + + // nanoseconds max, should be ok + constexpr auto nanoseconds_max = std::chrono::nanoseconds::max(); + EXPECT_NO_THROW( + rclcpp::GenericTimer( + steady_clock, nanoseconds_max, []() {}, context)); + + // 0 duration period, should be ok + EXPECT_NO_THROW( + rclcpp::GenericTimer(steady_clock, 0ms, []() {}, context)); + + // context is null, which resorts to default + EXPECT_NO_THROW( + rclcpp::GenericTimer(steady_clock, 1ms, []() {}, nullptr)); + + // Clock is unitialized + auto unitialized_clock = std::make_shared(RCL_CLOCK_UNINITIALIZED); + EXPECT_THROW( + rclcpp::GenericTimer(unitialized_clock, 1us, []() {}, context), + rclcpp::exceptions::RCLError); +} From 8e7e2a23ce9b8c831c36740f7cea76d3b7bc3539 Mon Sep 17 00:00:00 2001 From: tomoya Date: Wed, 24 Jun 2020 01:42:35 +0900 Subject: [PATCH 108/121] fix exception message on rcl_clock_init. (#1182) Signed-off-by: Tomoya.Fujita --- rclcpp/src/rclcpp/clock.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 4993df7f63..ed610b8ef8 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -32,7 +32,7 @@ class Clock::Impl { rcl_ret_t ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_); if (ret != RCL_RET_OK) { - exceptions::throw_from_rcl_error(ret, "could not get current time stamp"); + exceptions::throw_from_rcl_error(ret, "failed to initialize rcl clock"); } } From 54fab8b53efca3784f6b18264270ece27f8b561c Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Thu, 25 Jun 2020 09:52:19 -0400 Subject: [PATCH 109/121] Update tracetools' QL to 2 in rclcpp's QD (#1187) Signed-off-by: Christophe Bedard --- rclcpp/QUALITY_DECLARATION.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/QUALITY_DECLARATION.md b/rclcpp/QUALITY_DECLARATION.md index 83e024f20c..fb368f9ca2 100644 --- a/rclcpp/QUALITY_DECLARATION.md +++ b/rclcpp/QUALITY_DECLARATION.md @@ -193,7 +193,7 @@ It is **Quality Level 4**, see its [Quality Declaration document](https://github The `tracetools` package provides utilities for instrumenting the code in `rclcpp` so that it may be traced for debugging and performance analysis. -It is **Quality Level 4**, see its [Quality Declaration document](https://gitlab.com/micro-ROS/ros_tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md). +It is **Quality Level 2**, see its [Quality Declaration document](https://gitlab.com/micro-ROS/ros_tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] From 76e6ee5430ec3bebcad0a7cb5f2836efaf5958e1 Mon Sep 17 00:00:00 2001 From: Martijn Buijs Date: Fri, 26 Jun 2020 17:41:23 +0200 Subject: [PATCH 110/121] Include original exception in ComponentManagerException (#1157) * Include original exception in ComponentManagerException Signed-off-by: Martijn Buijs * Update rclcpp_components/src/component_manager.cpp Co-authored-by: tomoya Signed-off-by: Martijn Buijs Co-authored-by: tomoya --- rclcpp_components/src/component_manager.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index a80fada378..bfb1b5cf89 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -196,6 +196,11 @@ ComponentManager::OnLoadNode( try { node_wrappers_[node_id] = factory->create_node_instance(options); + } catch (const std::exception & ex) { + // In the case that the component constructor throws an exception, + // rethrow into the following catch block. + throw ComponentManagerException( + "Component constructor threw an exception: " + std::string(ex.what())); } catch (...) { // In the case that the component constructor throws an exception, // rethrow into the following catch block. From 46e4a1f7457f2bd1da2b2c6128e1c6995ce1fc1d Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 26 Jun 2020 20:00:14 -0300 Subject: [PATCH 111/121] Fix pub/sub count API tests. (#1203) Signed-off-by: Michel Hidalgo --- .../test_publisher_subscription_count_api.cpp | 265 +++++++++++------- .../test_subscription_publisher_count_api.cpp | 161 +++++++---- 2 files changed, 266 insertions(+), 160 deletions(-) diff --git a/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp b/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp index cbcd4f12ed..0a59ebe100 100644 --- a/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp +++ b/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp @@ -24,27 +24,92 @@ #include "test_msgs/msg/empty.hpp" -using test_msgs::msg::Empty; +namespace +{ -/** - * Parameterized test. - * The first param are the NodeOptions used to create the nodes. - * The second param are the expect intraprocess count results. - */ -struct TestParameters + +template +class NodeCreationPolicy { - rclcpp::NodeOptions node_options[2]; - uint64_t intraprocess_count_results[2]; - std::string description; +public: + rclcpp::NodeOptions & node_options() + { + return options_; + } + +private: + rclcpp::NodeOptions options_; }; -std::ostream & operator<<(std::ostream & out, const TestParameters & params) +template +class NodeCreationPolicy { - out << params.description; - return out; -} +public: + NodeCreationPolicy() + { + gather(options_); + } + + rclcpp::NodeOptions & node_options() + { + return options_; + } + +private: + template + static rclcpp::NodeOptions & + gather(rclcpp::NodeOptions & options) + { + return U::gather(options); + } + + template + static rclcpp::NodeOptions & + gather(rclcpp::NodeOptions & options) + { + return gather(U::gather(options)); + } + + rclcpp::NodeOptions options_; +}; + +template +struct ShouldUseIntraprocess +{ + static rclcpp::NodeOptions & gather(rclcpp::NodeOptions & options) + { + return options.use_intra_process_comms(value); + } +}; + +using UseIntraprocess = ShouldUseIntraprocess; +using DoNotUseIntraprocess = ShouldUseIntraprocess; + +struct UseCustomContext +{ + static rclcpp::NodeOptions & gather(rclcpp::NodeOptions & options) + { + auto context = rclcpp::Context::make_shared(); + context->init(0, nullptr); + return options.context(context); + } +}; -class TestPublisherSubscriptionCount : public ::testing::TestWithParam +struct PrintTestDescription +{ + template + static std::string GetName(int i) + { + static_cast(i); + return T::description; + } +}; + +} // namespace + + +template +class TestPublisherSubscriptionCount : public ::testing::Test { public: static void SetUpTestCase() @@ -57,126 +122,128 @@ class TestPublisherSubscriptionCount : public ::testing::TestWithParam; + using SecondNodeCreationPolicy = NodeCreationPolicy; - static std::chrono::milliseconds offset; + static constexpr bool first_node_talks_intraprocess{true}; + static constexpr bool both_nodes_talk_intraprocess{true}; }; -std::chrono::milliseconds TestPublisherSubscriptionCount::offset = std::chrono::milliseconds(2000); +/* Testing publisher subscription count api and internal process subscription count. + * Two subscriptions, one using intra-process comm and the other not using it. + */ +struct TwoSubscriptionsOneIntraprocessOneNot +{ + static constexpr const char * description = + "two_subscriptions_one_intraprocess_one_not"; + using FirstNodeCreationPolicy = NodeCreationPolicy; + using SecondNodeCreationPolicy = NodeCreationPolicy<>; + + static constexpr bool first_node_talks_intraprocess{true}; + static constexpr bool both_nodes_talk_intraprocess{false}; +}; -void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) +/* Testing publisher subscription count api and internal process subscription count. + * Two contexts, both using intra-process. + */ +struct TwoSubscriptionsInTwoContextsWithIntraprocessComm { - (void)msg; -} + static constexpr const char * description = + "two_subscriptions_in_two_contexts_with_intraprocess_comm"; + using FirstNodeCreationPolicy = NodeCreationPolicy; + using SecondNodeCreationPolicy = NodeCreationPolicy; + + static constexpr bool first_node_talks_intraprocess{true}; + static constexpr bool both_nodes_talk_intraprocess{false}; +}; -TEST_P(TestPublisherSubscriptionCount, increasing_and_decreasing_counts) +/* Testing publisher subscription count api and internal process subscription count. + * Two contexts, both of them not using intra-process comm. + */ +struct TwoSubscriptionsInTwoContextsWithoutIntraprocessComm { - TestParameters parameters = GetParam(); + static constexpr const char * description = + "two_subscriptions_in_two_contexts_without_intraprocess_comm"; + using FirstNodeCreationPolicy = NodeCreationPolicy<>; + using SecondNodeCreationPolicy = NodeCreationPolicy; + + static constexpr bool first_node_talks_intraprocess{false}; + static constexpr bool both_nodes_talk_intraprocess{false}; +}; + +using AllTestDescriptions = ::testing::Types< + TwoSubscriptionsIntraprocessComm, + TwoSubscriptionsOneIntraprocessOneNot, + TwoSubscriptionsInTwoContextsWithIntraprocessComm, + TwoSubscriptionsInTwoContextsWithoutIntraprocessComm +>; +TYPED_TEST_CASE(TestPublisherSubscriptionCount, AllTestDescriptions, PrintTestDescription); + + +using test_msgs::msg::Empty; + +TYPED_TEST(TestPublisherSubscriptionCount, increasing_and_decreasing_counts) +{ + using TestDescription = TypeParam; + typename TestDescription::FirstNodeCreationPolicy my_node_creation_policy; rclcpp::Node::SharedPtr node = std::make_shared( "my_node", "/ns", - parameters.node_options[0]); + my_node_creation_policy.node_options()); auto publisher = node->create_publisher("/topic", 10); EXPECT_EQ(publisher->get_subscription_count(), 0u); EXPECT_EQ(publisher->get_intra_process_subscription_count(), 0u); { - auto sub = node->create_subscription("/topic", 10, &OnMessage); - rclcpp::sleep_for(offset); + auto sub = node->create_subscription( + "/topic", 10, &TestPublisherSubscriptionCount::OnMessage); + rclcpp::sleep_for(this->offset); EXPECT_EQ(publisher->get_subscription_count(), 1u); EXPECT_EQ( publisher->get_intra_process_subscription_count(), - parameters.intraprocess_count_results[0]); + (TestDescription::first_node_talks_intraprocess ? 1u : 0u)); { + typename TestDescription::SecondNodeCreationPolicy another_node_creation_policy; 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); + another_node_creation_policy.node_options()); + auto another_sub = another_node->create_subscription( + "/topic", 10, &TestPublisherSubscriptionCount::OnMessage); - rclcpp::sleep_for(offset); + rclcpp::sleep_for(this->offset); EXPECT_EQ(publisher->get_subscription_count(), 2u); EXPECT_EQ( publisher->get_intra_process_subscription_count(), - parameters.intraprocess_count_results[1]); + (TestDescription::first_node_talks_intraprocess ? 1u : 0u) + + (TestDescription::both_nodes_talk_intraprocess ? 1u : 0u)); } - rclcpp::sleep_for(offset); + rclcpp::sleep_for(this->offset); EXPECT_EQ(publisher->get_subscription_count(), 1u); EXPECT_EQ( publisher->get_intra_process_subscription_count(), - parameters.intraprocess_count_results[0]); + (TestDescription::first_node_talks_intraprocess ? 1u : 0u)); } /** * Counts should be zero here, as all are subscriptions are out of scope. * Subscriptions count checking is always preceeded with an sleep, as random failures had been * detected without it. */ - rclcpp::sleep_for(offset); + rclcpp::sleep_for(this->offset); EXPECT_EQ(publisher->get_subscription_count(), 0u); EXPECT_EQ(publisher->get_intra_process_subscription_count(), 0u); } - -auto get_new_context() -{ - auto context = rclcpp::Context::make_shared(); - context->init(0, nullptr); - return context; -} - -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" - }, - /* - 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" - }, - /* - 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" - }, - /* - 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" - } -}; - -INSTANTIATE_TEST_CASE_P( - TestWithDifferentNodeOptions, TestPublisherSubscriptionCount, - ::testing::ValuesIn(parameters), - ::testing::PrintToStringParamName()); diff --git a/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp index e79b332a03..8da31f3060 100644 --- a/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp +++ b/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2019-2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,21 +23,79 @@ #include "test_msgs/msg/empty.hpp" -using test_msgs::msg::Empty; +namespace +{ -struct TestParameters +template +class NodeCreationPolicy { - rclcpp::NodeOptions node_options; - std::string description; +public: + rclcpp::NodeOptions & node_options() + { + return options_; + } + +private: + rclcpp::NodeOptions options_; }; -std::ostream & operator<<(std::ostream & out, const TestParameters & params) +template +class NodeCreationPolicy { - out << params.description; - return out; -} +public: + NodeCreationPolicy() + { + gather(options_); + } -class TestSubscriptionPublisherCount : public ::testing::TestWithParam + rclcpp::NodeOptions & node_options() + { + return options_; + } + +private: + template + static rclcpp::NodeOptions & + gather(rclcpp::NodeOptions & options) + { + return U::gather(options); + } + + template + static rclcpp::NodeOptions & + gather(rclcpp::NodeOptions & options) + { + return gather(U::gather(options)); + } + + rclcpp::NodeOptions options_; +}; + +struct UseCustomContext +{ + static rclcpp::NodeOptions & gather(rclcpp::NodeOptions & options) + { + auto context = rclcpp::Context::make_shared(); + context->init(0, nullptr); + return options.context(context); + } +}; + +struct PrintTestDescription +{ + template + static std::string GetName(int i) + { + static_cast(i); + return T::description; + } +}; + +} // namespace + + +template +class TestSubscriptionPublisherCount : public ::testing::Test { public: static void SetUpTestCase() @@ -50,78 +108,59 @@ class TestSubscriptionPublisherCount : public ::testing::TestWithParam; +}; -void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) +struct TwoContextsPerTest { - (void)msg; -} + static constexpr const char * description = "two_contexts_test"; + using NodeCreationPolicy = ::NodeCreationPolicy; +}; -TEST_P(TestSubscriptionPublisherCount, increasing_and_decreasing_counts) +using AllTestDescriptions = ::testing::Types; +TYPED_TEST_CASE(TestSubscriptionPublisherCount, AllTestDescriptions, PrintTestDescription); + + +using test_msgs::msg::Empty; + +TYPED_TEST(TestSubscriptionPublisherCount, increasing_and_decreasing_counts) { - rclcpp::NodeOptions node_options = GetParam().node_options; - rclcpp::Node::SharedPtr node = std::make_shared( - "my_node", - "/ns", - node_options); - auto subscription = node->create_subscription("/topic", 10, &OnMessage); + using TestDescription = TypeParam; + rclcpp::Node::SharedPtr node = std::make_shared("my_node", "/ns"); + auto subscription = node->create_subscription( + "/topic", 10, &TestSubscriptionPublisherCount::OnMessage); EXPECT_EQ(subscription->get_publisher_count(), 0u); { auto pub = node->create_publisher("/topic", 10); - rclcpp::sleep_for(offset); + rclcpp::sleep_for(this->offset); EXPECT_EQ(subscription->get_publisher_count(), 1u); { + typename TestDescription::NodeCreationPolicy node_creation_policy; rclcpp::Node::SharedPtr another_node = std::make_shared( "another_node", "/ns", - node_options); + node_creation_policy.node_options()); auto another_pub = another_node->create_publisher("/topic", 10); - rclcpp::sleep_for(offset); + rclcpp::sleep_for(this->offset); EXPECT_EQ(subscription->get_publisher_count(), 2u); } - rclcpp::sleep_for(offset); + rclcpp::sleep_for(this->offset); EXPECT_EQ(subscription->get_publisher_count(), 1u); } - rclcpp::sleep_for(offset); + rclcpp::sleep_for(this->offset); EXPECT_EQ(subscription->get_publisher_count(), 0u); } - -auto get_new_context() -{ - auto context = rclcpp::Context::make_shared(); - context->init(0, nullptr); - return context; -} - -TestParameters parameters[] = { - /* - Testing subscription publisher count api. - One context. - */ - { - rclcpp::NodeOptions(), - "one_context_test" - }, - /* - Testing subscription publisher count api. - Two contexts. - */ - { - rclcpp::NodeOptions().context(get_new_context()), - "two_contexts_test" - } -}; - -INSTANTIATE_TEST_CASE_P( - TestWithDifferentNodeOptions, - TestSubscriptionPublisherCount, - testing::ValuesIn(parameters), - testing::PrintToStringParamName()); From bdcd873954e1d4506118e766c17698b680714d42 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Fri, 26 Jun 2020 16:00:31 -0700 Subject: [PATCH 112/121] fix race in test_lifecycle_service_client (#1204) Signed-off-by: Dirk Thomas --- .../test/test_lifecycle_service_client.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp index 76978d052f..cc930eb0e0 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -217,21 +218,31 @@ class TestLifecycleServiceClient : public ::testing::Test void TearDown() override { - rclcpp::shutdown(); + { + std::lock_guard guard(shutdown_mutex_); + rclcpp::shutdown(); + } spinner_.join(); } void spin() { - while (rclcpp::ok()) { - rclcpp::spin_some(lifecycle_node_->get_node_base_interface()); - rclcpp::spin_some(lifecycle_client_); + while (true) { + { + std::lock_guard guard(shutdown_mutex_); + if (!rclcpp::ok()) { + break; + } + rclcpp::spin_some(lifecycle_node_->get_node_base_interface()); + rclcpp::spin_some(lifecycle_client_); + } std::this_thread::sleep_for(std::chrono::milliseconds(10)); } } std::shared_ptr lifecycle_node_; std::shared_ptr lifecycle_client_; + std::mutex shutdown_mutex_; std::thread spinner_; }; From 1f19f7c830792c011b9551f0ac159e383ef92f12 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 29 Jun 2020 08:43:34 +0200 Subject: [PATCH 113/121] Bump to QD to level 3 and fixed links (#1158) * Update quality level and links to doc Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde * Fixed wording and links Signed-off-by: ahcorde * Bump QD to level 3 and fixed links Signed-off-by: ahcorde * Added missing dependency rcpputils to rclcpp_components Signed-off-by: ahcorde * Added missing dependency rmw to rclcpp_lifecycle Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde * changed ci_linux_coverage to nightly_linux_coverage Signed-off-by: ahcorde --- rclcpp/QUALITY_DECLARATION.md | 28 +++++++++++---------- rclcpp/README.md | 2 +- rclcpp_action/QUALITY_DECLARATION.md | 20 ++++++++------- rclcpp_action/README.md | 2 +- rclcpp_components/QUALITY_DECLARATION.md | 32 ++++++++++++++---------- rclcpp_components/README.md | 2 +- rclcpp_lifecycle/QUALITY_DECLARATION.md | 30 ++++++++++++++-------- rclcpp_lifecycle/README.md | 2 +- 8 files changed, 68 insertions(+), 50 deletions(-) diff --git a/rclcpp/QUALITY_DECLARATION.md b/rclcpp/QUALITY_DECLARATION.md index fb368f9ca2..4077d39814 100644 --- a/rclcpp/QUALITY_DECLARATION.md +++ b/rclcpp/QUALITY_DECLARATION.md @@ -1,8 +1,8 @@ This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). -# `rclcpp` Quality Declaration +# rclcpp Quality Declaration -The package `rclcpp` claims to be in the **Quality Level 4** category. +The package `rclcpp` claims to be in the **Quality Level 3** category. Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. @@ -10,7 +10,7 @@ Below are the rationales, notes, and caveats for this claim, organized by each r ### Version Scheme [1.i] -`rclcpp` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning) +`rclcpp` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). ### Version Stability [1.ii] @@ -76,19 +76,19 @@ All pull requests must resolve related documentation changes before merging. ### Public API Documentation [3.ii] -The API is publicly available in the [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp/). +The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp/). ### License [3.iii] The license for `rclcpp` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file. -There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Epr/job/Epr__rclcpp__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package. +There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package. ### Copyright Statements [3.iv] The copyright holders each provide a statement of copyright in each source code file in `rclcpp`. -There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Epr/job/Epr__rclcpp__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp/copyright/). +There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp/copyright/). ## Testing [4] @@ -121,6 +121,8 @@ This includes: Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs). + ### Performance [4.iv] It is not yet defined if this package requires performance testing and how addresses this topic. @@ -151,43 +153,43 @@ It also has several test dependencies, which do not affect the resulting quality The `libstatistics_collector` package provides lightweight aggregation utilities to collect statistics and measure message metrics. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md). #### `rcl` `rcl` a library to support implementation of language specific ROS 2 Client Libraries. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md). #### `rcl_yaml_param_parser` The `rcl_yaml_param_parser` package provides an API that is used to parse YAML configuration files which may be used to configure ROS and specific nodes. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md). #### `rcpputils` The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md). +It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md). #### `rcutils` The `rcutils` package provides an API which contains common utilities and data structures useful when programming in C. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md). +It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md). #### `rmw` `rmw` is the ROS 2 middleware library. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md). +It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md). #### `statistics_msgs` The `statistics_msgs` package contains ROS 2 message definitions for reporting statistics for topics and system resources. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md). #### `tracetools` diff --git a/rclcpp/README.md b/rclcpp/README.md index dff8e5889c..02e86606a2 100644 --- a/rclcpp/README.md +++ b/rclcpp/README.md @@ -6,4 +6,4 @@ Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) fo ## Quality Declaration -This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. +This package claims to be in the **Quality Level 3** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rclcpp_action/QUALITY_DECLARATION.md b/rclcpp_action/QUALITY_DECLARATION.md index 97f4cb72ab..382edd4817 100644 --- a/rclcpp_action/QUALITY_DECLARATION.md +++ b/rclcpp_action/QUALITY_DECLARATION.md @@ -1,8 +1,8 @@ This document is a declaration of software quality for the `rclcpp_action` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). -# `rclcpp_action` Quality Declaration +# rclcpp_action Quality Declaration -The package `rclcpp_action` claims to be in the **Quality Level 4** category. +The package `rclcpp_action` claims to be in the **Quality Level 3** category. Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. @@ -10,7 +10,7 @@ Below are the rationales, notes, and caveats for this claim, organized by each r ### Version Scheme [1.i] -`rclcpp_action` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning) +`rclcpp_action` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). ### Version Stability [1.ii] @@ -74,19 +74,19 @@ All pull requests must resolve related documentation changes before merging. ### Public API Documentation [3.ii] -The API is publicly available in the [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp_action/). +The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp_action/). ### License [3.iii] The license for `rclcpp_action` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file. -There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_action__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_action/) can be found a list with the latest results of the various linters being run on the package. +There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp_action/) can be found a list with the latest results of the various linters being run on the package. ### Copyright Statements [3.iv] The copyright holders each provide a statement of copyright in each source code file in `rclcpp_action`. -There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_action__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_action/copyright/). +There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp_action/copyright/). ## Testing [4] @@ -119,6 +119,8 @@ This includes: Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rclcpp_rclcpp_action_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs). + ### Performance [4.iv] It is not yet defined if this package requires performance testing and how addresses this topic. @@ -149,19 +151,19 @@ It also has several test dependencies, which do not affect the resulting quality `action_msgs` provides messages and services for ROS 2 actions. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/QUALITY_DECLARATION.md). #### `rclcpp` The `rclcpp` package provides the ROS client library in C++. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). #### `rcl_action` The `rcl_action` package provides C-based ROS action implementation. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_action/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_action/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_action/README.md b/rclcpp_action/README.md index 45e14e7f3d..945a07285d 100644 --- a/rclcpp_action/README.md +++ b/rclcpp_action/README.md @@ -6,4 +6,4 @@ Visit the [rclcpp_action API documentation](http://docs.ros2.org/latest/api/rclc ## Quality Declaration -This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. +This package claims to be in the **Quality Level 3** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rclcpp_components/QUALITY_DECLARATION.md b/rclcpp_components/QUALITY_DECLARATION.md index 93d017edbf..6dd35f65a5 100644 --- a/rclcpp_components/QUALITY_DECLARATION.md +++ b/rclcpp_components/QUALITY_DECLARATION.md @@ -1,8 +1,8 @@ This document is a declaration of software quality for the `rclcpp_components` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). -# `rclcpp_components` Quality Declaration +# rclcpp_components Quality Declaration -The package `rclcpp_components` claims to be in the **Quality Level 4** category. +The package `rclcpp_components` claims to be in the **Quality Level 3** category. Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. @@ -10,7 +10,7 @@ Below are the rationales, notes, and caveats for this claim, organized by each r ### Version Scheme [1.i] -`rclcpp_components` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning) +`rclcpp_components` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). ### Version Stability [1.ii] @@ -70,25 +70,23 @@ All pull requests must resolve related documentation changes before merging. ### Feature Documentation [3.i] -`rclcpp_components` does not have a documented feature list. +`rclcpp_components` has a [feature list](http://docs.ros2.org/latest/api/rclcpp_components/) and each item in the list links to the corresponding feature documentation. There is documentation for all of the features, and new features require documentation before being added. ### Public API Documentation [3.ii] -`rclcpp_components` does not cover a public API documentation. - -The API is publicly available in the [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp_components/). +The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp_components/). ### License [3.iii] The license for `rclcpp_components` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file. -There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_components__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_components/) can be found a list with the latest results of the various linters being run on the package. +There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp_components/) can be found a list with the latest results of the various linters being run on the package. ### Copyright Statements [3.iv] The copyright holders each provide a statement of copyright in each source code file in `rclcpp_components`. -There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_components__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_components/copyright/). +There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp_components/copyright). ## Testing [4] @@ -121,6 +119,8 @@ This includes: Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rclcpp_rclcpp_components_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs). + ### Performance [4.iv] It is not yet defined if this package requires performance testing and how addresses this topic. @@ -151,25 +151,31 @@ It also has several test dependencies, which do not affect the resulting quality The `ament_index_cpp` package provides a C++ API to access the ament resource index. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ament/ament_index/blob/master/ament_index_cpp/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ament/ament_index/blob/master/ament_index_cpp/QUALITY_DECLARATION.md). #### `class_loader` The `class_loader` package provides a ROS-independent package for loading plugins during runtime -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros/class_loader/blob/ros2/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros/class_loader/blob/ros2/QUALITY_DECLARATION.md). #### `composition_interfaces` The `composition_interfaces` package contains message and service definitions for managing composable nodes in a container process. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/composition_interfaces/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/composition_interfaces/QUALITY_DECLARATION.md). #### `rclcpp` The `rclcpp` package provides the ROS client library in C++. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). + +#### `rcpputils` + +The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++. + +It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_components/README.md b/rclcpp_components/README.md index 9abeef1d1e..0c33d1c51f 100644 --- a/rclcpp_components/README.md +++ b/rclcpp_components/README.md @@ -6,4 +6,4 @@ Visit the [rclcpp_components API documentation](http://docs.ros2.org/latest/api/ ## Quality Declaration -This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. +This package claims to be in the **Quality Level 3** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rclcpp_lifecycle/QUALITY_DECLARATION.md b/rclcpp_lifecycle/QUALITY_DECLARATION.md index 4452f39e13..a06cce5575 100644 --- a/rclcpp_lifecycle/QUALITY_DECLARATION.md +++ b/rclcpp_lifecycle/QUALITY_DECLARATION.md @@ -1,8 +1,8 @@ This document is a declaration of software quality for the `rclcpp_lifecycle` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). -# `rclcpp_lifecycle` Quality Declaration +# rclcpp_lifecycle Quality Declaration -The package `rclcpp_lifecycle` claims to be in the **Quality Level 4** category. +The package `rclcpp_lifecycle` claims to be in the **Quality Level 3** category. Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. @@ -10,7 +10,7 @@ Below are the rationales, notes, and caveats for this claim, organized by each r ### Version Scheme [1.i] -`rclcpp_lifecycle` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning) +`rclcpp_lifecycle` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). ### Version Stability [1.ii] @@ -70,23 +70,23 @@ All pull requests must resolve related documentation changes before merging. ### Feature Documentation [3.i] -`rclcpp_lifecycle` does not have a documented feature list. +`rclcpp_lifecycle` has a [feature list](http://docs.ros2.org/latest/api/rclcpp_lifecycle/index.html) and each item in the list links to the corresponding feature documentation. There is documentation for all of the features, and new features require documentation before being added. ### Public API Documentation [3.ii] -`rclcpp_lifecycle` does not cover a public API documentation. +The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp_lyfecycle/). ### License [3.iii] The license for `rclcpp_lifecycle` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file. -There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_lifecycle__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_lifecycle/) can be found a list with the latest results of the various linters being run on the package. +There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp_lifecycle/) can be found a list with the latest results of the various linters being run on the package. ### Copyright Statements [3.iv] The copyright holders each provide a statement of copyright in each source code file in `rclcpp_lifecycle`. -There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Epr/job/Epr__rclcpp_lifecycle__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp_lifecycle/copyright/). +There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp_lifecycle/copyright/). ## Testing [4] @@ -119,6 +119,8 @@ This includes: Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rclcpp_rclcpp_lifecycle_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs). + ### Performance [4.iv] It is not yet defined if this package requires performance testing and how addresses this topic. @@ -149,25 +151,31 @@ It also has several test dependencies, which do not affect the resulting quality The `lifecycle_msgs` package contains message and service definitions for managing lifecycle nodes. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/QUALITY_DECLARATION.md). #### `rclcpp` The `rclcpp` package provides the ROS client library in C++. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). #### `rcl_lifecycle` The `rcl_lifecycle` package provides functionality for ROS 2 lifecycle nodes in C. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_lifecycle/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_lifecycle/QUALITY_DECLARATION.md). #### `rosidl_typesupport_cpp` The `rosidl_typesupport_cpp` package generates the type support for C++ messages. -It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rosidl_typesupport/blob/master/rosidl_typesupport_cpp/QUALITY_DECLARATION.md). +It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rosidl_typesupport/blob/master/rosidl_typesupport_cpp/QUALITY_DECLARATION.md). + +#### `rmw` + +`rmw` is the ROS 2 middleware library. + +It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_lifecycle/README.md b/rclcpp_lifecycle/README.md index b4e51e5dff..8058f4f338 100644 --- a/rclcpp_lifecycle/README.md +++ b/rclcpp_lifecycle/README.md @@ -6,4 +6,4 @@ Visit the [rclcpp_lifecycle API documentation](http://docs.ros2.org/latest/api/r ## Quality Declaration -This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. +This package claims to be in the **Quality Level 3** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. From dcb1a856e4f4bace5ab5ce442d5fbcbc7257684e Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Mon, 29 Jun 2020 15:47:23 +0200 Subject: [PATCH 114/121] Fix implementation of NodeOptions::use_global_arguments() (#1176) `this->node_options_` might still be `nullptr` for a default initialized NodeOptions instance. `use_global_arguments()` must return `this->use_global_arguments_`, in analogy to `NodeOptions::enable_rosout()`. Signed-off-by: Johannes Meyer --- rclcpp/src/rclcpp/node_options.cpp | 2 +- rclcpp/test/rclcpp/test_node_options.cpp | 32 ++++++++++++++++++++++++ 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index cac10e6685..0d9303b6ef 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -176,7 +176,7 @@ NodeOptions::parameter_overrides(const std::vector & paramete bool NodeOptions::use_global_arguments() const { - return this->node_options_->use_global_arguments; + return this->use_global_arguments_; } NodeOptions & diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index a90cfd1f02..17d108f92e 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -105,6 +105,38 @@ TEST(TestNodeOptions, bad_ros_args) { rclcpp::exceptions::UnknownROSArgsError); } +TEST(TestNodeOptions, use_global_arguments) { + { + auto options = rclcpp::NodeOptions(); + EXPECT_TRUE(options.use_global_arguments()); + EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments); + } + + { + auto options = rclcpp::NodeOptions().use_global_arguments(false); + EXPECT_FALSE(options.use_global_arguments()); + EXPECT_FALSE(options.get_rcl_node_options()->use_global_arguments); + } + + { + auto options = rclcpp::NodeOptions().use_global_arguments(true); + EXPECT_TRUE(options.use_global_arguments()); + EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments); + } + + { + auto options = rclcpp::NodeOptions(); + EXPECT_TRUE(options.use_global_arguments()); + EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments); + options.use_global_arguments(false); + EXPECT_FALSE(options.use_global_arguments()); + EXPECT_FALSE(options.get_rcl_node_options()->use_global_arguments); + options.use_global_arguments(true); + EXPECT_TRUE(options.use_global_arguments()); + EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments); + } +} + TEST(TestNodeOptions, enable_rosout) { { auto options = rclcpp::NodeOptions(); From 0634cc27e7f05096b6022b6d1bcb8ffad5b4a6ac Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Mon, 29 Jun 2020 17:44:19 +0200 Subject: [PATCH 115/121] Fix conversion of negative durations to messages (#1188) * Fix conversion from negative Duration or Time to the respective message type and throw in Duration::to_rmw_time() if the duration is negative. rmw_time_t cannot represent negative durations. Constructors and assignment operators can be just defaulted. Other changes are mainly cosmetical, to make conversions between signed and unsigned types and between 32-bit and 64-bit types more explicit. Signed-off-by: Johannes Meyer * Add -Wconversion compiler option and fix implicit conversions that might alter the value Signed-off-by: Johannes Meyer * Fix usage of fixture class in some unit tests by using gtest macro TEST_F() instead of TEST(). Signed-off-by: Johannes Meyer * Add compiler option -Wno-sign-conversion to fix build with Clang on macOS Signed-off-by: Johannes Meyer --- rclcpp/CMakeLists.txt | 6 +- rclcpp/include/rclcpp/duration.hpp | 2 +- rclcpp/include/rclcpp/time.hpp | 14 +- rclcpp/src/rclcpp/duration.cpp | 34 ++-- .../node_interfaces/node_parameters.cpp | 2 +- rclcpp/src/rclcpp/node_options.cpp | 2 +- rclcpp/src/rclcpp/time.cpp | 38 ++--- .../test_multi_threaded_executor.cpp | 2 +- rclcpp/test/rclcpp/test_duration.cpp | 105 +++++++++++-- rclcpp/test/rclcpp/test_parameter.cpp | 28 ++-- rclcpp/test/rclcpp/test_time.cpp | 146 ++++++++++++++---- 11 files changed, 272 insertions(+), 107 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 81db9a365d..8230400b77 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -23,7 +23,11 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) + # About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of + # implicit sign conversions in rclcpp and gtest.cc, see https://ci.ros2.org/job/ci_osx/9265/. + # Hence disabling -Wsign-conversion for now until all those have eventually been fixed. + # (from https://github.com/ros2/rclcpp/pull/1188#issuecomment-650229140) + add_compile_options(-Wall -Wextra -Wconversion -Wno-sign-conversion -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) endif() set(${PROJECT_NAME}_SRCS diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index eb716c3348..d5f8401cbb 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -72,7 +72,7 @@ class RCLCPP_PUBLIC Duration operator=(const Duration & rhs); Duration & - operator=(const builtin_interfaces::msg::Duration & Duration_msg); + operator=(const builtin_interfaces::msg::Duration & duration_msg); bool operator==(const rclcpp::Duration & rhs) const; diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index 6782393b4c..af41bef435 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -48,10 +48,10 @@ class Time /// Time constructor /** * \param nanoseconds since time epoch - * \param clock clock type + * \param clock_type clock type */ RCLCPP_PUBLIC - explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME); + explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); /// Copy constructor RCLCPP_PUBLIC @@ -60,13 +60,13 @@ class Time /// Time constructor /** * \param time_msg builtin_interfaces time message to copy - * \param ros_time clock type + * \param clock_type clock type * \throws std::runtime_error if seconds are negative */ RCLCPP_PUBLIC Time( const builtin_interfaces::msg::Time & time_msg, - rcl_clock_type_t ros_time = RCL_ROS_TIME); + rcl_clock_type_t clock_type = RCL_ROS_TIME); /// Time constructor /** @@ -90,6 +90,12 @@ class Time Time & operator=(const Time & rhs); + /** + * Assign Time from a builtin_interfaces::msg::Time instance. + * The clock_type will be reset to RCL_ROS_TIME. + * Equivalent to *this = Time(time_msg, RCL_ROS_TIME). + * \throws std::runtime_error if seconds are negative + */ RCLCPP_PUBLIC Time & operator=(const builtin_interfaces::msg::Time & time_msg); diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 2114ddd16f..cd335e68e4 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -47,17 +47,14 @@ Duration::Duration(std::chrono::nanoseconds nanoseconds) rcl_duration_.nanoseconds = nanoseconds.count(); } -Duration::Duration(const Duration & rhs) -{ - rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds; -} +Duration::Duration(const Duration & rhs) = default; Duration::Duration( const builtin_interfaces::msg::Duration & duration_msg) { rcl_duration_.nanoseconds = - static_cast(RCL_S_TO_NS(duration_msg.sec)); - rcl_duration_.nanoseconds += duration_msg.nanosec; + RCL_S_TO_NS(static_cast(duration_msg.sec)); + rcl_duration_.nanoseconds += static_cast(duration_msg.nanosec); } Duration::Duration(const rcl_duration_t & duration) @@ -69,24 +66,25 @@ Duration::Duration(const rcl_duration_t & duration) Duration::operator builtin_interfaces::msg::Duration() const { builtin_interfaces::msg::Duration msg_duration; - msg_duration.sec = static_cast(RCL_NS_TO_S(rcl_duration_.nanoseconds)); - msg_duration.nanosec = - static_cast(rcl_duration_.nanoseconds % (1000 * 1000 * 1000)); + constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1); + const auto result = std::div(rcl_duration_.nanoseconds, kDivisor); + if (result.rem >= 0) { + msg_duration.sec = static_cast(result.quot); + msg_duration.nanosec = static_cast(result.rem); + } else { + msg_duration.sec = static_cast(result.quot - 1); + msg_duration.nanosec = static_cast(kDivisor + result.rem); + } return msg_duration; } Duration & -Duration::operator=(const Duration & rhs) -{ - rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds; - return *this; -} +Duration::operator=(const Duration & rhs) = default; 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; + *this = Duration(duration_msg); return *this; } @@ -230,6 +228,10 @@ Duration::seconds() const rmw_time_t Duration::to_rmw_time() const { + if (rcl_duration_.nanoseconds < 0) { + throw std::runtime_error("rmw_time_t cannot be negative"); + } + // reuse conversion logic from msg creation builtin_interfaces::msg::Duration msg = *this; rmw_time_t result; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index f5d0aa7e64..412fba46c5 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -159,7 +159,7 @@ __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) +__are_doubles_equal(double x, double y, double ulp = 100.0) { return std::abs(x - y) <= std::numeric_limits::epsilon() * std::abs(x + y) * ulp; } diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 0d9303b6ef..3a2dcd7f5c 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -338,7 +338,7 @@ NodeOptions::get_domain_id_from_env() const _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var); #endif if (ros_domain_id) { - uint32_t number = strtoul(ros_domain_id, NULL, 0); + uint32_t number = static_cast(strtoul(ros_domain_id, NULL, 0)); if (number == (std::numeric_limits::max)()) { #ifdef _WIN32 // free the ros_domain_id before throwing, if getenv was used on Windows diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index 63289161ee..e9d633e046 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -63,17 +63,13 @@ 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_) -{ - rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds; -} +Time::Time(const Time & rhs) = default; Time::Time( const builtin_interfaces::msg::Time & time_msg, - rcl_clock_type_t ros_time) + rcl_clock_type_t clock_type) +: rcl_time_(init_time_point(clock_type)) { - rcl_time_ = init_time_point(ros_time); if (time_msg.sec < 0) { throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); } @@ -95,31 +91,25 @@ Time::~Time() Time::operator builtin_interfaces::msg::Time() const { builtin_interfaces::msg::Time msg_time; - msg_time.sec = static_cast(RCL_NS_TO_S(rcl_time_.nanoseconds)); - msg_time.nanosec = static_cast(rcl_time_.nanoseconds % (1000 * 1000 * 1000)); + constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1); + const auto result = std::div(rcl_time_.nanoseconds, kRemainder); + if (result.rem >= 0) { + msg_time.sec = static_cast(result.quot); + msg_time.nanosec = static_cast(result.rem); + } else { + msg_time.sec = static_cast(result.quot - 1); + msg_time.nanosec = static_cast(kRemainder + result.rem); + } return msg_time; } Time & -Time::operator=(const Time & rhs) -{ - rcl_time_ = rhs.rcl_time_; - return *this; -} +Time::operator=(const Time & rhs) = default; 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 - - rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast(time_msg.sec)); - rcl_time_.nanoseconds += time_msg.nanosec; + *this = Time(time_msg); return *this; } diff --git a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp index bfe2b25457..b6fd2c3fda 100644 --- a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp @@ -83,7 +83,7 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { { std::lock_guard lock(last_mutex); - double diff = std::abs((now - last).nanoseconds()) / 1.0e9; + double diff = static_cast(std::abs((now - last).nanoseconds())) / 1.0e9; last = now; if (diff < PERIOD - TOLERANCE) { diff --git a/rclcpp/test/rclcpp/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp index b69a790d28..4217005baa 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -32,11 +32,7 @@ class TestDuration : public ::testing::Test { }; -// TEST(TestDuration, conversions) { -// TODO(tfoote) Implement conversion methods -// } - -TEST(TestDuration, operators) { +TEST_F(TestDuration, operators) { rclcpp::Duration old(1, 0); rclcpp::Duration young(2, 0); @@ -67,7 +63,7 @@ TEST(TestDuration, operators) { EXPECT_TRUE(time == assignment_op_duration); } -TEST(TestDuration, chrono_overloads) { +TEST_F(TestDuration, chrono_overloads) { int64_t ns = 123456789l; auto chrono_ns = std::chrono::nanoseconds(ns); auto d1 = rclcpp::Duration(ns); @@ -86,7 +82,7 @@ TEST(TestDuration, chrono_overloads) { EXPECT_EQ(chrono_float_seconds, d5.to_chrono()); } -TEST(TestDuration, overflows) { +TEST_F(TestDuration, overflows) { rclcpp::Duration max(std::numeric_limits::max()); rclcpp::Duration min(std::numeric_limits::min()); @@ -107,7 +103,7 @@ TEST(TestDuration, overflows) { EXPECT_THROW(base_d_neg * 4, std::underflow_error); } -TEST(TestDuration, negative_duration) { +TEST_F(TestDuration, negative_duration) { rclcpp::Duration assignable_duration = rclcpp::Duration(0) - rclcpp::Duration(5, 0); { @@ -130,7 +126,7 @@ TEST(TestDuration, negative_duration) { } } -TEST(TestDuration, maximum_duration) { +TEST_F(TestDuration, maximum_duration) { rclcpp::Duration max_duration = rclcpp::Duration::max(); rclcpp::Duration max(std::numeric_limits::max(), 999999999); @@ -138,18 +134,105 @@ TEST(TestDuration, maximum_duration) { } static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000; +static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000; static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS; -TEST(TestDuration, from_seconds) { +TEST_F(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_F(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)); EXPECT_EQ(rclcpp::Duration(-1, 0), rclcpp::Duration(-1s)); } + +TEST_F(TestDuration, conversions) { + { + const rclcpp::Duration duration(HALF_SEC_IN_NS); + const auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, 0); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), HALF_SEC_IN_NS); + + const auto rmw_time = duration.to_rmw_time(); + EXPECT_EQ(rmw_time.sec, 0u); + EXPECT_EQ(rmw_time.nsec, static_cast(HALF_SEC_IN_NS)); + + const auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), HALF_SEC_IN_NS); + } + + { + const rclcpp::Duration duration(ONE_SEC_IN_NS); + const auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, 1); + EXPECT_EQ(duration_msg.nanosec, 0u); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), ONE_SEC_IN_NS); + + const auto rmw_time = duration.to_rmw_time(); + EXPECT_EQ(rmw_time.sec, 1u); + EXPECT_EQ(rmw_time.nsec, 0u); + + const auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), ONE_SEC_IN_NS); + } + + { + const rclcpp::Duration duration(ONE_AND_HALF_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, 1); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), ONE_AND_HALF_SEC_IN_NS); + + auto rmw_time = duration.to_rmw_time(); + EXPECT_EQ(rmw_time.sec, 1u); + EXPECT_EQ(rmw_time.nsec, static_cast(HALF_SEC_IN_NS)); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), ONE_AND_HALF_SEC_IN_NS); + } + + { + rclcpp::Duration duration(-HALF_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, -1); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -HALF_SEC_IN_NS); + + EXPECT_THROW(duration.to_rmw_time(), std::runtime_error); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), -HALF_SEC_IN_NS); + } + + { + rclcpp::Duration duration(-ONE_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, -1); + EXPECT_EQ(duration_msg.nanosec, 0u); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -ONE_SEC_IN_NS); + + EXPECT_THROW(duration.to_rmw_time(), std::runtime_error); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), -ONE_SEC_IN_NS); + } + + { + rclcpp::Duration duration(-ONE_AND_HALF_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, -2); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -ONE_AND_HALF_SEC_IN_NS); + + EXPECT_THROW(duration.to_rmw_time(), std::runtime_error); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS); + } +} diff --git a/rclcpp/test/rclcpp/test_parameter.cpp b/rclcpp/test/rclcpp/test_parameter.cpp index 876fbb4e93..f7244e4488 100644 --- a/rclcpp/test/rclcpp/test_parameter.cpp +++ b/rclcpp/test/rclcpp/test_parameter.cpp @@ -32,7 +32,7 @@ class TestParameter : public ::testing::Test } }; -TEST(TestParameter, not_set_variant) { +TEST_F(TestParameter, not_set_variant) { // Direct instantiation rclcpp::Parameter not_set_variant; EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, not_set_variant.get_type()); @@ -58,7 +58,7 @@ TEST(TestParameter, not_set_variant) { rclcpp::Parameter::from_parameter_msg(not_set_param).get_type()); } -TEST(TestParameter, bool_variant) { +TEST_F(TestParameter, bool_variant) { // Direct instantiation rclcpp::Parameter bool_variant_true("bool_param", true); EXPECT_EQ("bool_param", bool_variant_true.get_name()); @@ -116,7 +116,7 @@ TEST(TestParameter, bool_variant) { bool_variant_false.get_value_message().type); } -TEST(TestParameter, integer_variant) { +TEST_F(TestParameter, integer_variant) { const int TEST_VALUE {42}; // Direct instantiation @@ -164,7 +164,7 @@ TEST(TestParameter, integer_variant) { from_msg.get_value_message().type); } -TEST(TestParameter, long_integer_variant) { +TEST_F(TestParameter, long_integer_variant) { const int64_t TEST_VALUE {std::numeric_limits::max()}; // Direct instantiation @@ -212,7 +212,7 @@ TEST(TestParameter, long_integer_variant) { from_msg.get_value_message().type); } -TEST(TestParameter, float_variant) { +TEST_F(TestParameter, float_variant) { const float TEST_VALUE {42.0f}; // Direct instantiation @@ -260,7 +260,7 @@ TEST(TestParameter, float_variant) { from_msg.get_value_message().type); } -TEST(TestParameter, double_variant) { +TEST_F(TestParameter, double_variant) { const double TEST_VALUE {-42.1}; // Direct instantiation @@ -308,7 +308,7 @@ TEST(TestParameter, double_variant) { from_msg.get_value_message().type); } -TEST(TestParameter, string_variant) { +TEST_F(TestParameter, string_variant) { const std::string TEST_VALUE {"ROS2"}; // Direct instantiation @@ -354,7 +354,7 @@ TEST(TestParameter, string_variant) { from_msg.get_value_message().type); } -TEST(TestParameter, byte_array_variant) { +TEST_F(TestParameter, byte_array_variant) { const std::vector TEST_VALUE {0x52, 0x4f, 0x53, 0x32}; // Direct instantiation @@ -402,7 +402,7 @@ TEST(TestParameter, byte_array_variant) { from_msg.get_value_message().type); } -TEST(TestParameter, bool_array_variant) { +TEST_F(TestParameter, bool_array_variant) { const std::vector TEST_VALUE {false, true, true, false, false, true}; // Direct instantiation @@ -450,7 +450,7 @@ TEST(TestParameter, bool_array_variant) { from_msg.get_value_message().type); } -TEST(TestParameter, integer_array_variant) { +TEST_F(TestParameter, integer_array_variant) { const std::vector TEST_VALUE {42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; @@ -529,7 +529,7 @@ TEST(TestParameter, integer_array_variant) { rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); } -TEST(TestParameter, long_integer_array_variant) { +TEST_F(TestParameter, long_integer_array_variant) { const std::vector TEST_VALUE {42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; @@ -583,7 +583,7 @@ TEST(TestParameter, long_integer_array_variant) { from_msg.get_value_message().type); } -TEST(TestParameter, float_array_variant) { +TEST_F(TestParameter, float_array_variant) { const std::vector TEST_VALUE {42.1f, -99.1f, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1f}; @@ -662,7 +662,7 @@ TEST(TestParameter, float_array_variant) { from_msg.get_value_message().type); } -TEST(TestParameter, double_array_variant) { +TEST_F(TestParameter, double_array_variant) { const std::vector TEST_VALUE {42.1, -99.1, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1}; @@ -716,7 +716,7 @@ TEST(TestParameter, double_array_variant) { from_msg.get_value_message().type); } -TEST(TestParameter, string_array_variant) { +TEST_F(TestParameter, string_array_variant) { const std::vector TEST_VALUE {"R", "O", "S2"}; // Direct instantiation diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index 830c0319e7..ac7f8e0a80 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -45,7 +45,7 @@ class TestTime : public ::testing::Test } }; -TEST(TestTime, clock_type_access) { +TEST_F(TestTime, clock_type_access) { rclcpp::Clock ros_clock(RCL_ROS_TIME); EXPECT_EQ(RCL_ROS_TIME, ros_clock.get_clock_type()); @@ -57,7 +57,7 @@ TEST(TestTime, clock_type_access) { } // Check that the clock may go out of the scope before the jump callback without leading in UB. -TEST(TestTime, clock_jump_callback_destruction_order) { +TEST_F(TestTime, clock_jump_callback_destruction_order) { rclcpp::JumpHandler::SharedPtr handler; { rclcpp::Clock ros_clock(RCL_ROS_TIME); @@ -68,7 +68,7 @@ TEST(TestTime, clock_jump_callback_destruction_order) { } } -TEST(TestTime, time_sources) { +TEST_F(TestTime, time_sources) { using builtin_interfaces::msg::Time; rclcpp::Clock ros_clock(RCL_ROS_TIME); Time ros_now = ros_clock.now(); @@ -86,44 +86,124 @@ TEST(TestTime, time_sources) { EXPECT_NE(0u, steady_now.nanosec); } -TEST(TestTime, conversions) { +static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000; +static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000; +static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS; + +TEST_F(TestTime, conversions) { rclcpp::Clock system_clock(RCL_SYSTEM_TIME); - rclcpp::Time now = system_clock.now(); - builtin_interfaces::msg::Time now_msg = now; + { + rclcpp::Time now = system_clock.now(); + builtin_interfaces::msg::Time now_msg = now; + + rclcpp::Time now_again = now_msg; + EXPECT_EQ(now.nanoseconds(), now_again.nanoseconds()); + } - rclcpp::Time now_again = now_msg; - EXPECT_EQ(now.nanoseconds(), now_again.nanoseconds()); + { + rclcpp::Time positive_time = rclcpp::Time(12345, 67890u); + + builtin_interfaces::msg::Time msg = positive_time; + EXPECT_EQ(msg.sec, 12345); + EXPECT_EQ(msg.nanosec, 67890u); + + rclcpp::Time time = msg; + EXPECT_EQ(time.nanoseconds(), positive_time.nanoseconds()); + EXPECT_EQ( + RCL_S_TO_NS(static_cast(msg.sec)) + static_cast(msg.nanosec), + time.nanoseconds()); + EXPECT_EQ(static_cast(msg.sec), RCL_NS_TO_S(time.nanoseconds())); + } + + // throw on construction/assignment of negative times + { + builtin_interfaces::msg::Time negative_time_msg; + 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(-1, 1)); + + EXPECT_ANY_THROW( + { + rclcpp::Time assignment(1, 2); + assignment = negative_time_msg; + }); + } - builtin_interfaces::msg::Time msg; - msg.sec = 12345; - msg.nanosec = 67890; + { + const rclcpp::Time time(HALF_SEC_IN_NS); + const auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, 0); + EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), HALF_SEC_IN_NS); + } - rclcpp::Time time = msg; - EXPECT_EQ( - RCL_S_TO_NS(static_cast(msg.sec)) + static_cast(msg.nanosec), - time.nanoseconds()); - EXPECT_EQ(static_cast(msg.sec), RCL_NS_TO_S(time.nanoseconds())); + { + const rclcpp::Time time(ONE_SEC_IN_NS); + const auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, 1); + EXPECT_EQ(time_msg.nanosec, 0u); + EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), ONE_SEC_IN_NS); + } - builtin_interfaces::msg::Time negative_time_msg; - negative_time_msg.sec = -1; - negative_time_msg.nanosec = 1; + { + const rclcpp::Time time(ONE_AND_HALF_SEC_IN_NS); + auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, 1); + EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), ONE_AND_HALF_SEC_IN_NS); + } - EXPECT_ANY_THROW( { - rclcpp::Time negative_time = negative_time_msg; - }); + // Can rclcpp::Time be negative or not? The following constructor works: + rclcpp::Time time(-HALF_SEC_IN_NS); + auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, -1); + EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); + + // The opposite conversion throws... + EXPECT_ANY_THROW( + { + rclcpp::Time negative_time(time_msg); + }); + } - EXPECT_ANY_THROW(rclcpp::Time(-1, 1)); + { + // Can rclcpp::Time be negative or not? The following constructor works: + rclcpp::Time time(-ONE_SEC_IN_NS); + auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, -1); + EXPECT_EQ(time_msg.nanosec, 0u); + + // The opposite conversion throws... + EXPECT_ANY_THROW( + { + rclcpp::Time negative_time(time_msg); + }); + } - EXPECT_ANY_THROW( { - rclcpp::Time assignment(1, 2); - assignment = negative_time_msg; - }); + // Can rclcpp::Time be negative or not? The following constructor works: + rclcpp::Time time(-ONE_AND_HALF_SEC_IN_NS); + auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, -2); + EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); + + // The opposite conversion throws... + EXPECT_ANY_THROW( + { + rclcpp::Time negative_time(time_msg); + }); + } } -TEST(TestTime, operators) { +TEST_F(TestTime, operators) { rclcpp::Time old(1, 0); rclcpp::Time young(2, 0); @@ -178,7 +258,7 @@ TEST(TestTime, operators) { } } -TEST(TestTime, overflow_detectors) { +TEST_F(TestTime, overflow_detectors) { ///////////////////////////////////////////////////////////////////////////// // Test logical_eq call first: EXPECT_TRUE(logical_eq(false, false)); @@ -198,8 +278,8 @@ TEST(TestTime, overflow_detectors) { // 256 * 256 = 64K total loops, should be pretty fast on everything for (big_type_t y = min_val; y <= max_val; ++y) { for (big_type_t x = min_val; x <= max_val; ++x) { - const big_type_t sum = x + y; - const big_type_t diff = x - y; + const big_type_t sum = static_cast(x + y); + const big_type_t diff = static_cast(x - y); const bool add_will_overflow = rclcpp::add_will_overflow(test_type_t(x), test_type_t(y)); @@ -235,7 +315,7 @@ TEST(TestTime, overflow_detectors) { EXPECT_TRUE(rclcpp::sub_will_underflow(INT64_MIN, 1)); } -TEST(TestTime, overflows) { +TEST_F(TestTime, overflows) { rclcpp::Time max_time(std::numeric_limits::max()); rclcpp::Time min_time(std::numeric_limits::min()); rclcpp::Duration one(1); @@ -267,7 +347,7 @@ TEST(TestTime, overflows) { EXPECT_NO_THROW(one_time - two_time); } -TEST(TestTime, seconds) { +TEST_F(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()); From 42b6354ea5f194a271a74b7792d4dcf8a54cd274 Mon Sep 17 00:00:00 2001 From: Claire Wang <22240514+claireyywang@users.noreply.github.com> Date: Mon, 29 Jun 2020 15:24:12 -0700 Subject: [PATCH 116/121] remove deprecated set_on_parameters_set_callback function (#1199) Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- rclcpp/include/rclcpp/node.hpp | 20 -- .../node_interfaces/node_parameters.hpp | 5 - .../node_parameters_interface.hpp | 11 - rclcpp/src/rclcpp/node.cpp | 22 -- .../node_interfaces/node_parameters.cpp | 12 -- rclcpp/test/rclcpp/test_node.cpp | 200 ------------------ .../rclcpp_lifecycle/lifecycle_node.hpp | 11 - rclcpp_lifecycle/src/lifecycle_node.cpp | 22 -- 8 files changed, 303 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 08e42bd634..ba0c719383 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -842,26 +842,6 @@ class Node : public std::enable_shared_from_this void remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler); - /// Register a callback to be called anytime a parameter is about to be changed. - /** - * \deprecated Use add_on_set_parameters_callback instead. - * With this method, only one callback can be set at a time. The callback that was previously - * set by this method is returned or `nullptr` if no callback was previously set. - * - * The callbacks added with `add_on_set_parameters_callback` are stored in a different place. - * `remove_on_set_parameters_callback` can't be used with the callbacks registered with this - * method. For removing it, use `set_on_parameters_set_callback(nullptr)`. - * - * \param[in] callback The callback to be called when the value for a - * parameter is about to be set. - * \return The previous callback that was registered, if there was one, - * otherwise nullptr. - */ - [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]] - RCLCPP_PUBLIC - OnParametersSetCallbackType - set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback); - /// Get the fully-qualified names of all available nodes. /** * The fully-qualified name includes the local namespace and name of the node. diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index cf2676810a..4d70508eb7 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -167,11 +167,6 @@ class NodeParameters : public NodeParametersInterface void remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override; - [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]] - RCLCPP_PUBLIC - OnParametersSetCallbackType - set_on_parameters_set_callback(OnParametersSetCallbackType callback) override; - RCLCPP_PUBLIC const std::map & get_parameter_overrides() const override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index c44763a09a..5b73998dbd 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -191,17 +191,6 @@ class NodeParametersInterface void remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0; - /// Register a callback for when parameters are being set, return an existing one. - /** - * \deprecated Use add_on_set_parameters_callback instead. - * \sa rclcpp::Node::set_on_parameters_set_callback - */ - [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]] - RCLCPP_PUBLIC - 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 diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 778d33ed01..b69d7c2baf 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -328,28 +328,6 @@ Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * co return node_parameters_->remove_on_set_parameters_callback(callback); } -// suppress deprecated function warning -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - -rclcpp::Node::OnParametersSetCallbackType -Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) -{ - return node_parameters_->set_on_parameters_set_callback(callback); -} - -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif - std::vector Node::get_node_names() const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 412fba46c5..90c851be90 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -874,18 +874,6 @@ NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callb return handle; } -NodeParameters::OnParametersSetCallbackType -NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback) -{ - std::lock_guard lock(mutex_); - - ParameterMutationRecursionGuard guard(parameter_modification_enabled_); - - auto existing_callback = on_parameters_set_callback_; - on_parameters_set_callback_ = callback; - return existing_callback; -} - const std::map & NodeParameters::get_parameter_overrides() const { diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index f5cd2a2ae9..b3ff08d700 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -2329,206 +2329,6 @@ TEST_F(TestNode, get_parameter_types_undeclared_parameters_allowed) { } } -// suppress deprecated function test warnings -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - -// test that it is possible to call get_parameter within the set_callback -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); - EXPECT_EQ(intval, 42); - double floatval = node->declare_parameter("floatparam", 5.4); - 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; - } - - 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(); - - return result; - }; - - EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); - ASSERT_NO_THROW(node->set_parameter({"intparam", 40})); - ASSERT_EQ(floatout, 5.4); -} - -// test that calling set_parameter inside of a set_callback throws an exception -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); - EXPECT_EQ(intval, 42); - 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; - } - - // This should throw an exception - node->set_parameter({"floatparam", 5.6}); - - 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); -} - -// test that calling declare_parameter inside of a set_callback throws an exception -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); - EXPECT_EQ(intval, 42); - 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; - } - - // This should throw an exception - node->declare_parameter("floatparam2", 5.6); - - 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); -} - -// test that calling undeclare_parameter inside a set_callback throws an exception -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); - EXPECT_EQ(intval, 42); - 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; - } - - // This should throw an exception - node->undeclare_parameter("floatparam"); - - 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); -} - -// 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) { - auto node = std::make_shared("test_set_callback_set_callback_node"_unq); - - int64_t intval = node->declare_parameter("intparam", 42); - EXPECT_EQ(intval, 42); - 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; - }; - - // 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); -} - -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif - void expect_qos_profile_eq( const rmw_qos_profile_t & qos1, const rmw_qos_profile_t & qos2, bool is_publisher) { diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index bd2327fb97..9a7d6c977d 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -469,17 +469,6 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, remove_on_set_parameters_callback( const rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle * const handler); - /// Register a callback to be called anytime a parameter is about to be changed. - /** - * \deprecated Use add_on_set_parameters_callback instead. - * \sa rclcpp::Node::set_on_parameters_set_callback - */ - [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]] - RCLCPP_LIFECYCLE_PUBLIC - rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType - set_on_parameters_set_callback( - rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType callback); - /// Return a vector of existing node names (string). /** * \sa rclcpp::Node::get_node_names diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 1d7210546c..4da5289917 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -268,28 +268,6 @@ LifecycleNode::remove_on_set_parameters_callback( node_parameters_->remove_on_set_parameters_callback(callback); } -// suppress deprecated function warning -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - -rclcpp::Node::OnParametersSetCallbackType -LifecycleNode::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) -{ - return node_parameters_->set_on_parameters_set_callback(callback); -} - -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif - std::vector LifecycleNode::get_node_names() const { From fb3657bbcc467c871b0f6a3ef30575d25a5f087b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 1 Jul 2020 13:16:56 -0300 Subject: [PATCH 117/121] Remove usage of domain id in node options (#1205) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/node_options.hpp | 5 ---- rclcpp/src/rclcpp/node_options.cpp | 33 -------------------------- 2 files changed, 38 deletions(-) diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 01b0158759..4be29968f7 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -327,11 +327,6 @@ class NodeOptions 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; - private: // This is mutable to allow for a const accessor which lazily creates the node options instance. /// Underlying rcl_node_options structure. diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 3a2dcd7f5c..96afaa9481 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -92,7 +92,6 @@ NodeOptions::get_rcl_node_options() const *node_options_ = rcl_node_get_default_options(); node_options_->allocator = this->allocator_; node_options_->use_global_arguments = this->use_global_arguments_; - node_options_->domain_id = this->get_domain_id_from_env(); node_options_->enable_rosout = this->enable_rosout_; int c_argc = 0; @@ -322,36 +321,4 @@ NodeOptions::allocator(rcl_allocator_t allocator) return *this; } -// 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 -{ - // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable. - size_t domain_id = std::numeric_limits::max(); - char * ros_domain_id = nullptr; - const char * env_var = "ROS_DOMAIN_ID"; -#ifndef _WIN32 - ros_domain_id = getenv(env_var); -#else - size_t ros_domain_id_size; - _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var); -#endif - if (ros_domain_id) { - uint32_t number = static_cast(strtoul(ros_domain_id, NULL, 0)); - if (number == (std::numeric_limits::max)()) { -#ifdef _WIN32 - // free the ros_domain_id before throwing, if getenv was used on Windows - free(ros_domain_id); -#endif - throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number"); - } - domain_id = static_cast(number); -#ifdef _WIN32 - free(ros_domain_id); -#endif - } - return domain_id; -} - } // namespace rclcpp From 60cf113c21c9d6fade25508a362bf6314f90547a Mon Sep 17 00:00:00 2001 From: brawner Date: Wed, 1 Jul 2020 12:48:41 -0700 Subject: [PATCH 118/121] Unit tests for node interfaces (#1202) * Unit tests for node interfaces Signed-off-by: Stephen Brawner * Address PR Feedback Signed-off-by: Stephen Brawner * Address PR feedback Signed-off-by: Stephen Brawner * Adjusting comment Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 46 +++++ .../rclcpp/node_interfaces/test_node_base.cpp | 60 ++++++ .../node_interfaces/test_node_clock.cpp | 49 +++++ .../node_interfaces/test_node_graph.cpp | 186 ++++++++++++++++++ .../node_interfaces/test_node_parameters.cpp | 89 +++++++++ .../node_interfaces/test_node_services.cpp | 111 +++++++++++ .../node_interfaces/test_node_timers.cpp | 71 +++++++ .../node_interfaces/test_node_topics.cpp | 129 ++++++++++++ .../node_interfaces/test_node_waitables.cpp | 68 +++++++ 9 files changed, 809 insertions(+) create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 46fe7c15d6..974b3c7c35 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -136,6 +136,52 @@ ament_add_gtest(test_node_interfaces__get_node_interfaces if(TARGET test_node_interfaces__get_node_interfaces) target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME}) endif() +ament_add_gtest(test_node_interfaces__node_base + rclcpp/node_interfaces/test_node_base.cpp) +if(TARGET test_node_interfaces__node_base) + target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_clock + rclcpp/node_interfaces/test_node_clock.cpp) +if(TARGET test_node_interfaces__node_clock) + target_link_libraries(test_node_interfaces__node_clock ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_graph + rclcpp/node_interfaces/test_node_graph.cpp) +if(TARGET test_node_interfaces__node_graph) + ament_target_dependencies( + test_node_interfaces__node_graph + "test_msgs") + target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_parameters + rclcpp/node_interfaces/test_node_parameters.cpp) +if(TARGET test_node_interfaces__node_parameters) + target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_services + rclcpp/node_interfaces/test_node_services.cpp) +if(TARGET test_node_interfaces__node_services) + target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_timers + rclcpp/node_interfaces/test_node_timers.cpp) +if(TARGET test_node_interfaces__node_timers) + target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_topics + rclcpp/node_interfaces/test_node_topics.cpp) +if(TARGET test_node_interfaces__node_topics) + ament_target_dependencies( + test_node_interfaces__node_topics + "test_msgs") + target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_waitables + rclcpp/node_interfaces/test_node_waitables.cpp) +if(TARGET test_node_interfaces__node_waitables) + target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME}) +endif() # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp new file mode 100644 index 0000000000..cfe42d7819 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp @@ -0,0 +1,60 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNodeBase : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeBase, construct_from_node) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_base = + dynamic_cast(node->get_node_base_interface().get()); + ASSERT_NE(nullptr, node_base); + + EXPECT_STREQ("node", node_base->get_name()); + EXPECT_STREQ("/ns", node_base->get_namespace()); + + EXPECT_STREQ("/ns/node", node_base->get_fully_qualified_name()); + EXPECT_NE(nullptr, node_base->get_context()); + EXPECT_NE(nullptr, node_base->get_rcl_node_handle()); + EXPECT_NE(nullptr, node_base->get_shared_rcl_node_handle()); + + const auto * const_node_base = node_base; + EXPECT_NE(nullptr, const_node_base->get_rcl_node_handle()); + EXPECT_NE(nullptr, const_node_base->get_shared_rcl_node_handle()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp new file mode 100644 index 0000000000..dec171b655 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp @@ -0,0 +1,49 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/node_interfaces/node_clock.hpp" +#include "rclcpp/node.hpp" + +class TestNodeClock : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeClock, construct_from_node) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_clock = + dynamic_cast(node->get_node_clock_interface().get()); + ASSERT_NE(nullptr, node_clock); + EXPECT_NE(nullptr, node_clock->get_clock()); + + const auto * const_node_clock = node_clock; + EXPECT_NE(nullptr, const_node_clock->get_clock()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp new file mode 100644 index 0000000000..db69207f3b --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -0,0 +1,186 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/node_interfaces/node_graph.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/empty.hpp" + +class TestNodeGraph : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeGraph, construct_from_node) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + const auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + auto topic_names_and_types = node_graph->get_topic_names_and_types(false); + EXPECT_LT(0u, topic_names_and_types.size()); + + auto service_names_and_types = node_graph->get_service_names_and_types(); + EXPECT_LT(0u, service_names_and_types.size()); + + auto names = node_graph->get_node_names(); + EXPECT_EQ(1u, names.size()); + + auto names_and_namespaces = node_graph->get_node_names_and_namespaces(); + EXPECT_EQ(1u, names_and_namespaces.size()); + + EXPECT_EQ(0u, node_graph->count_publishers("not_a_topic")); + EXPECT_EQ(0u, node_graph->count_subscribers("not_a_topic")); +} + +TEST_F(TestNodeGraph, get_topic_names_and_types) +{ + auto node = std::make_shared("node2", "ns"); + const auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + auto topic_names_and_types = node_graph->get_topic_names_and_types(); + EXPECT_LT(0u, topic_names_and_types.size()); +} + +TEST_F(TestNodeGraph, get_service_names_and_types) +{ + auto node = std::make_shared("node2", "ns"); + const auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + auto service_names_and_types = node_graph->get_service_names_and_types(); + EXPECT_LT(0u, service_names_and_types.size()); +} + +TEST_F(TestNodeGraph, get_service_names_and_types_by_node) +{ + auto node1 = std::make_shared("node1", "ns"); + auto node2 = std::make_shared("node2", "ns"); + const auto * node_graph = + dynamic_cast(node1->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + EXPECT_THROW( + node_graph->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + std::runtime_error); + auto service_names_and_types1 = node_graph->get_service_names_and_types_by_node("node1", "/ns"); + auto service_names_and_types2 = node_graph->get_service_names_and_types_by_node("node2", "/ns"); + EXPECT_EQ(service_names_and_types1.size(), service_names_and_types2.size()); +} + +TEST_F(TestNodeGraph, get_node_names_and_namespaces) +{ + auto node = std::make_shared("node", "ns"); + const auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + auto names_and_namespaces = node_graph->get_node_names_and_namespaces(); + EXPECT_EQ(1u, names_and_namespaces.size()); +} + +TEST_F(TestNodeGraph, notify_shutdown) +{ + auto node = std::make_shared("node", "ns"); + auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + EXPECT_NO_THROW(node_graph->notify_shutdown()); +} + +TEST_F(TestNodeGraph, wait_for_graph_change) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + EXPECT_NO_THROW(node_graph->notify_graph_change()); + EXPECT_THROW( + node_graph->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)), + rclcpp::exceptions::InvalidEventError); + + auto event = std::make_shared(); + EXPECT_THROW( + node_graph->wait_for_graph_change(event, std::chrono::milliseconds(0)), + rclcpp::exceptions::EventNotRegisteredError); +} + +TEST_F(TestNodeGraph, get_info_by_topic) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + const rclcpp::QoS publisher_qos(1); + auto publisher = node->create_publisher("topic", publisher_qos); + auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + + const rclcpp::QoS subscriber_qos(10); + auto subscription = + node->create_subscription( + "topic", subscriber_qos, std::move(callback)); + + const auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + auto publishers = node_graph->get_publishers_info_by_topic("topic", false); + ASSERT_EQ(1u, publishers.size()); + + auto publisher_endpoint_info = publishers[0]; + const auto const_publisher_endpoint_info = publisher_endpoint_info; + EXPECT_STREQ("node", publisher_endpoint_info.node_name().c_str()); + EXPECT_STREQ("node", const_publisher_endpoint_info.node_name().c_str()); + EXPECT_STREQ("/ns", publisher_endpoint_info.node_namespace().c_str()); + EXPECT_STREQ("/ns", const_publisher_endpoint_info.node_namespace().c_str()); + EXPECT_STREQ("test_msgs/msg/Empty", publisher_endpoint_info.topic_type().c_str()); + EXPECT_STREQ("test_msgs/msg/Empty", const_publisher_endpoint_info.topic_type().c_str()); + EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_endpoint_info.endpoint_type()); + EXPECT_EQ(rclcpp::EndpointType::Publisher, const_publisher_endpoint_info.endpoint_type()); + + rclcpp::QoS actual_qos = publisher_endpoint_info.qos_profile(); + EXPECT_EQ(0u, actual_qos.get_rmw_qos_profile().depth); + + rclcpp::QoS const_actual_qos = const_publisher_endpoint_info.qos_profile(); + EXPECT_EQ(0u, const_actual_qos.get_rmw_qos_profile().depth); + + auto endpoint_gid = publisher_endpoint_info.endpoint_gid(); + auto const_endpoint_gid = const_publisher_endpoint_info.endpoint_gid(); + bool endpoint_gid_is_all_zeros = true; + for (size_t i = 0; i < RMW_GID_STORAGE_SIZE; ++i) { + endpoint_gid_is_all_zeros &= (endpoint_gid[i] == 0); + EXPECT_EQ(endpoint_gid[i], const_endpoint_gid[i]); + } + EXPECT_FALSE(endpoint_gid_is_all_zeros); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp new file mode 100644 index 0000000000..68a7546c83 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -0,0 +1,89 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** + * NodeParameters is a complicated interface with lots of code, but it is tested elsewhere + * very thoroughly. This currently just includes unittests for the currently uncovered + * functionality. + */ + +#include + +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_parameters.hpp" + +class TestNodeParameters : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeParameters, list_parameters) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + std::vector prefixes; + const auto list_result = node_parameters->list_parameters(prefixes, 1u); + + // Currently the only default parameter is 'use_sim_time', but that may change. + size_t number_of_parameters = list_result.names.size(); + EXPECT_GE(1u, number_of_parameters); + + const std::string parameter_name = "new_parameter"; + const rclcpp::ParameterValue value(true); + const rcl_interfaces::msg::ParameterDescriptor descriptor; + const auto added_parameter_value = + node_parameters->declare_parameter(parameter_name, value, descriptor, false); + EXPECT_EQ(value.get(), added_parameter_value.get()); + + auto list_result2 = node_parameters->list_parameters(prefixes, 1u); + EXPECT_EQ(number_of_parameters + 1u, list_result2.names.size()); + + EXPECT_NE( + std::find(list_result2.names.begin(), list_result2.names.end(), parameter_name), + list_result2.names.end()); +} + +TEST_F(TestNodeParameters, parameter_overrides) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(0u, parameter_overrides.size()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp new file mode 100644 index 0000000000..893340d33a --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -0,0 +1,111 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_services.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestService : public rclcpp::ServiceBase +{ +public: + explicit TestService(rclcpp::Node * node) + : rclcpp::ServiceBase(node->get_node_base_interface()->get_shared_rcl_node_handle()) {} + + std::shared_ptr create_request() override {return nullptr;} + std::shared_ptr create_request_header() override {return nullptr;} + void handle_request(std::shared_ptr, std::shared_ptr) override {} +}; + +class TestClient : public rclcpp::ClientBase +{ +public: + explicit TestClient(rclcpp::Node * node) + : rclcpp::ClientBase(node->get_node_base_interface().get(), node->get_node_graph_interface()) {} + + std::shared_ptr create_response() override {return nullptr;} + std::shared_ptr create_request_header() override {return nullptr;} + void handle_response( + std::shared_ptr, std::shared_ptr) override {} +}; + +class TestNodeService : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeService, add_service) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_services = + dynamic_cast( + node->get_node_services_interface().get()); + ASSERT_NE(nullptr, node_services); + + auto service = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW( + node_services->add_service(service, callback_group)); + + // Check that adding a service from node to a callback group of different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_services->add_service(service, callback_group_in_different_node), + std::runtime_error); +} + +TEST_F(TestNodeService, add_client) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_services = + dynamic_cast( + node->get_node_services_interface().get()); + ASSERT_NE(nullptr, node_services); + + auto client = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_services->add_client(client, callback_group)); + + // Check that adding a client from node to a callback group of different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_services->add_client(client, callback_group_in_different_node), + std::runtime_error); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp new file mode 100644 index 0000000000..af340d292e --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -0,0 +1,71 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_timers.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestTimer : public rclcpp::TimerBase +{ +public: + explicit TestTimer(rclcpp::Node * node) + : TimerBase(node->get_clock(), std::chrono::nanoseconds(1), + node->get_node_base_interface()->get_context()) {} + + void execute_callback() override {} + bool is_steady() override {return false;} +}; + +class TestNodeTimers : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeTimers, add_timer) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto node_timers = + dynamic_cast(node->get_node_timers_interface().get()); + ASSERT_NE(nullptr, node_timers); + auto timer = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_timers->add_timer(timer, callback_group)); + + // Check that adding timer from node to callback group in different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_timers->add_timer(timer, callback_group_in_different_node), + std::runtime_error); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp new file mode 100644 index 0000000000..86b4e72e36 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -0,0 +1,129 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/empty.hpp" + +namespace +{ + +const rosidl_message_type_support_t EmptyTypeSupport() +{ + return *rosidl_typesupport_cpp::get_message_type_support_handle(); +} + +const rcl_publisher_options_t PublisherOptions() +{ + return rclcpp::PublisherOptionsWithAllocator>().template + to_rcl_publisher_options(rclcpp::QoS(10)); +} + +const rcl_subscription_options_t SubscriptionOptions() +{ + return rclcpp::SubscriptionOptionsWithAllocator>().template + to_rcl_subscription_options(rclcpp::QoS(10)); +} + +} // namespace + +class TestPublisher : public rclcpp::PublisherBase +{ +public: + explicit TestPublisher(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {} +}; + +class TestSubscription : public rclcpp::SubscriptionBase +{ +public: + explicit TestSubscription(rclcpp::Node * node) + : rclcpp::SubscriptionBase( + node->get_node_base_interface().get(), EmptyTypeSupport(), "topic", SubscriptionOptions()) {} + std::shared_ptr create_message() override {return nullptr;} + + std::shared_ptr + create_serialized_message() override {return nullptr;} + + void handle_message(std::shared_ptr &, const rclcpp::MessageInfo &) override {} + void handle_loaned_message(void *, const rclcpp::MessageInfo &) override {} + void return_message(std::shared_ptr &) override {} + void return_serialized_message(std::shared_ptr &) override {} +}; + +class TestNodeTopics : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeTopics, add_publisher) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_topics = + dynamic_cast(node->get_node_topics_interface().get()); + ASSERT_NE(nullptr, node_topics); + auto publisher = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_topics->add_publisher(publisher, callback_group)); + + // Check that adding publisher from node to a callback group in different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_topics->add_publisher(publisher, callback_group_in_different_node), + std::runtime_error); +} + +TEST_F(TestNodeTopics, add_subscription) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + auto * node_topics = + dynamic_cast(node->get_node_topics_interface().get()); + ASSERT_NE(nullptr, node_topics); + auto subscription = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_topics->add_subscription(subscription, callback_group)); + + // Check that adding subscription from node to callback group in different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_topics->add_subscription(subscription, callback_group_in_different_node), + std::runtime_error); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp new file mode 100644 index 0000000000..19cc63fe07 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -0,0 +1,68 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_waitables.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestWaitable : public rclcpp::Waitable +{ +public: + bool add_to_wait_set(rcl_wait_set_t *) override {return false;} + bool is_ready(rcl_wait_set_t *) override {return false;} + void execute() override {} +}; + +class TestNodeWaitables : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeWaitables, add_remove_waitable) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + auto * node_waitables = + dynamic_cast( + node->get_node_waitables_interface().get()); + ASSERT_NE(nullptr, node_waitables); + + std::shared_ptr node2 = std::make_shared("node2", "ns"); + + auto callback_group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto callback_group2 = node2->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto waitable = std::make_shared(); + EXPECT_NO_THROW( + node_waitables->add_waitable(waitable, callback_group1)); + EXPECT_THROW( + node_waitables->add_waitable(waitable, callback_group2), + std::runtime_error); + EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group1)); + EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group2)); +} From edd59413aef24177f71e6d62892cb3be20d9fd6f Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 1 Jul 2020 13:22:33 -0700 Subject: [PATCH 119/121] link against thread library where necessary (#1210) Signed-off-by: Dirk Thomas --- rclcpp/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 8230400b77..26d7e69362 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.5) project(rclcpp) +find_package(Threads REQUIRED) + find_package(ament_cmake_ros REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(libstatistics_collector REQUIRED) @@ -170,6 +172,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$" "$" "$") +target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT}) # specific order: dependents before dependencies ament_target_dependencies(${PROJECT_NAME} "libstatistics_collector" From 6b41e9ce2c9f0430f2ba9c8ef1e156f6b9a0b139 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 2 Jul 2020 16:08:40 -0300 Subject: [PATCH 120/121] Fix rclcpp::NodeOptions::operator= (#1211) Signed-off-by: Michel Hidalgo --- rclcpp/src/rclcpp/node_options.cpp | 1 + rclcpp/test/rclcpp/test_node_options.cpp | 30 ++++++++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 96afaa9481..179c5bffe7 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -79,6 +79,7 @@ NodeOptions::operator=(const NodeOptions & other) this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_; this->automatically_declare_parameters_from_overrides_ = other.automatically_declare_parameters_from_overrides_; + this->node_options_.reset(); } return *this; } diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 17d108f92e..8543d9b975 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -168,3 +168,33 @@ TEST(TestNodeOptions, enable_rosout) { EXPECT_TRUE(options.get_rcl_node_options()->enable_rosout); } } + +TEST(TestNodeOptions, copy) { + std::vector expected_args{"--unknown-flag", "arg"}; + auto options = rclcpp::NodeOptions().arguments(expected_args).use_global_arguments(false); + const rcl_node_options_t * rcl_options = options.get_rcl_node_options(); + + { + rclcpp::NodeOptions copied_options = options; + EXPECT_FALSE(copied_options.use_global_arguments()); + EXPECT_EQ(expected_args, copied_options.arguments()); + const rcl_node_options_t * copied_rcl_options = copied_options.get_rcl_node_options(); + EXPECT_EQ(copied_rcl_options->use_global_arguments, rcl_options->use_global_arguments); + EXPECT_EQ( + rcl_arguments_get_count_unparsed(&copied_rcl_options->arguments), + rcl_arguments_get_count_unparsed(&rcl_options->arguments)); + } + + { + auto other_options = rclcpp::NodeOptions().use_global_arguments(true); + (void)other_options.get_rcl_node_options(); // force C structure initialization + other_options = options; + EXPECT_FALSE(other_options.use_global_arguments()); + EXPECT_EQ(expected_args, other_options.arguments()); + const rcl_node_options_t * other_rcl_options = other_options.get_rcl_node_options(); + EXPECT_EQ(other_rcl_options->use_global_arguments, rcl_options->use_global_arguments); + EXPECT_EQ( + rcl_arguments_get_count_unparsed(&other_rcl_options->arguments), + rcl_arguments_get_count_unparsed(&rcl_options->arguments)); + } +} From b858281fd0c5096a758cbf026196d5311b2bd309 Mon Sep 17 00:00:00 2001 From: DensoADAS <46967124+DensoADAS@users.noreply.github.com> Date: Mon, 27 Jul 2020 07:05:58 +0200 Subject: [PATCH 121/121] Update rclcpp/include/rclcpp/experimental/intra_process_manager.hpp Co-authored-by: Karsten Knese --- rclcpp/include/rclcpp/experimental/intra_process_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 39a5d16aa1..dcbb3ca20f 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -309,7 +309,7 @@ class IntraProcessManager }; /// get the index for "take_subscriptions" depending on shared/serialized - constexpr static auto + constexpr static size_t get_index(const bool is_shared, const bool is_serialized) { return (is_serialized ? IndexSharedTyped : IndexSharedSerialized) +