From 12b939cf5a80fe221742aa510da0148c5a4dd622 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 19 Aug 2015 13:10:15 -0700 Subject: [PATCH] fixes to address comments and CI failures --- rclcpp/CMakeLists.txt | 4 +- rclcpp/include/rclcpp/context.hpp | 2 + .../include/rclcpp/intra_process_manager.hpp | 54 ++-- rclcpp/include/rclcpp/mapped_ring_buffer.hpp | 31 ++- rclcpp/include/rclcpp/node.hpp | 8 +- rclcpp/include/rclcpp/node_impl.hpp | 20 +- rclcpp/include/rclcpp/publisher.hpp | 6 +- rclcpp/include/rclcpp/subscription.hpp | 6 +- rclcpp/test/test_intra_process_manager.cpp | 248 ++++++++++-------- rclcpp/test/test_mapped_ring_buffer.cpp | 66 +++-- 10 files changed, 264 insertions(+), 181 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 1501ef8..2cc262d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -22,7 +22,9 @@ if(AMENT_ENABLE_TESTING) ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp) ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp) - target_include_directories(test_intra_process_manager PUBLIC "${rcl_interfaces_INCLUDE_DIRS}") + if(TARGET test_intra_process_manager) + target_include_directories(test_intra_process_manager PUBLIC "${rcl_interfaces_INCLUDE_DIRS}") + endif() endif() ament_package( diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index f859a7f..1c56cbb 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -51,11 +51,13 @@ public: auto it = sub_contexts_.find(type_i); if (it == sub_contexts_.end()) { // It doesn't exist yet, make it + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) sub_context = std::shared_ptr( new SubContext(std::forward(args) ...), [] (SubContext * sub_context_ptr) { delete sub_context_ptr; }); + // *INDENT-ON* sub_contexts_[type_i] = sub_context; } else { // It exists, get it out and cast it. diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/intra_process_manager.hpp index 38eee41..e689731 100644 --- a/rclcpp/include/rclcpp/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/intra_process_manager.hpp @@ -21,8 +21,9 @@ #include #include -#include +#include #include +#include #include #include #include @@ -115,6 +116,7 @@ class IntraProcessManager { private: RCLCPP_DISABLE_COPY(IntraProcessManager); + public: RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager); @@ -129,8 +131,8 @@ public: * * This method will allocate memory. * - * /param subscription the Subscription to register. - * /return an unsigned 64-bit integer which is the subscription's unique id. + * \param subscription the Subscription to register. + * \return an unsigned 64-bit integer which is the subscription's unique id. */ uint64_t add_subscription(subscription::SubscriptionBase::SharedPtr subscription) @@ -144,7 +146,7 @@ public: /// Unregister a subscription using the subscription's unique id. /* This method does not allocate memory. * - * /param intra_process_subscription_id id of the subscription to remove. + * \param intra_process_subscription_id id of the subscription to remove. */ void remove_subscription(uint64_t intra_process_subscription_id) @@ -180,19 +182,21 @@ public: * * This method will allocate memory. * - * /param publisher publisher to be registered with the manager. - * /param buffer_size if 0 (default) a size is calculated based on the QoS. - * /return an unsigned 64-bit integer which is the publisher's unique id. + * \param publisher publisher to be registered with the manager. + * \param buffer_size if 0 (default) a size is calculated based on the QoS. + * \return an unsigned 64-bit integer which is the publisher's unique id. */ template uint64_t - add_publisher(publisher::Publisher::SharedPtr publisher, size_t buffer_size=0) + add_publisher(publisher::Publisher::SharedPtr publisher, size_t buffer_size = 0) { auto id = IntraProcessManager::get_next_unique_id(); publishers_[id].publisher = publisher; size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size(); // As long as the size of the ring buffer is less than the max sequence number, we're safe. - assert(size <= std::numeric_limits::max()); + if (size > std::numeric_limits::max()) { + throw std::invalid_argument("the calculated buffer size is too large"); + } publishers_[id].sequence_number.store(0); publishers_[id].buffer = mapped_ring_buffer::MappedRingBuffer::make_shared(size); publishers_[id].target_subscriptions_by_message_sequence.reserve(size); @@ -202,7 +206,7 @@ public: /// Unregister a publisher using the publisher's unique id. /* This method does not allocate memory. * - * /param intra_process_publisher_id id of the publisher to remove. + * \param intra_process_publisher_id id of the publisher to remove. */ void remove_publisher(uint64_t intra_process_publisher_id) @@ -236,9 +240,9 @@ public: * * This method does allocate memory. * - * /param intra_process_publisher_id the id of the publisher of this message. - * /param message the message that is being stored. - * /return the message sequence number. + * \param intra_process_publisher_id the id of the publisher of this message. + * \param message the message that is being stored. + * \return the message sequence number. */ template uint64_t @@ -250,7 +254,7 @@ public: if (it == publishers_.end()) { throw std::runtime_error("store_intra_process_message called with invalid publisher id"); } - publisher_info & info = it->second; + PublisherInfo & info = it->second; // Calculate the next message sequence number. uint64_t message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed); // Insert the message into the ring buffer using the message_seq to identify it. @@ -309,10 +313,10 @@ public: * * This method may allocate memory to copy the stored message. * - * /param intra_process_publisher_id the id of the message's publisher. - * /param message_sequence_number the sequence number of the message. - * /param requesting_subscriptions_intra_process_id the subscription's id. - * /param message the message typed unique_ptr used to return the message. + * \param intra_process_publisher_id the id of the message's publisher. + * \param message_sequence_number the sequence number of the message. + * \param requesting_subscriptions_intra_process_id the subscription's id. + * \param message the message typed unique_ptr used to return the message. */ template void @@ -323,7 +327,7 @@ public: std::unique_ptr & message) { message = nullptr; - publisher_info * info; + PublisherInfo * info; { auto it = publishers_.find(intra_process_publisher_id); if (it == publishers_.end()) { @@ -377,9 +381,11 @@ private: // So around 585 million years. Even at 1 GHz, it would take 585 years. // I think it's safe to avoid trying to handle overflow. // If we roll over then it's most likely a bug. + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::overflow_error( "exhausted the unique id's for publishers and subscribers in this process " "(congratulations your computer is either extremely fast or extremely old)"); + // *INDENT-ON* } return next_id; } @@ -389,11 +395,11 @@ private: std::unordered_map subscriptions_; std::map> subscription_ids_by_topic_; - struct publisher_info + struct PublisherInfo { - RCLCPP_DISABLE_COPY(publisher_info); + RCLCPP_DISABLE_COPY(PublisherInfo); - publisher_info() = default; + PublisherInfo() = default; publisher::Publisher::WeakPtr publisher; std::atomic sequence_number; @@ -401,11 +407,11 @@ private: std::unordered_map> target_subscriptions_by_message_sequence; }; - std::unordered_map publishers_; + std::unordered_map publishers_; }; -std::atomic IntraProcessManager::next_unique_id_{1}; +std::atomic IntraProcessManager::next_unique_id_ {1}; } /* namespace intra_process_manager */ } /* namespace rclcpp */ diff --git a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp index d4fc0f7..4b5bb47 100644 --- a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp +++ b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp @@ -59,9 +59,10 @@ public: /// Constructor. /* The constructor will allocate memory while reserving space. * - * /param size size of the ring buffer; must be positive and non-zero. + * \param size size of the ring buffer; must be positive and non-zero. */ - MappedRingBuffer(size_t size) : elements_(size), head_(0) + MappedRingBuffer(size_t size) + : elements_(size), head_(0) { if (size == 0) { throw std::invalid_argument("size must be a positive, non-zero value"); @@ -75,8 +76,10 @@ public: * * The key is not guaranteed to be unique, see the class docs for more. * - * /param key the key associated with the stored value - * /param value if the key is found, the value is stored in this parameter + * The contents of value before the method is called are discarded. + * + * \param key the key associated with the stored value + * \param value if the key is found, the value is stored in this parameter */ void get_copy_at_key(uint64_t key, std::unique_ptr & value) @@ -102,8 +105,10 @@ public: * originally stored object, since it was returned by the first call to this * method. * - * /param key the key associated with the stored value - * /param value if the key is found, the value is stored in this parameter + * The contents of value before the method is called are discarded. + * + * \param key the key associated with the stored value + * \param value if the key is found, the value is stored in this parameter */ void get_ownership_at_key(uint64_t key, std::unique_ptr & value) @@ -125,8 +130,10 @@ public: * * The key is not guaranteed to be unique, see the class docs for more. * - * /param key the key associated with the stored value - * /param value if the key is found, the value is stored in this parameter + * The contents of value before the method is called are discarded. + * + * \param key the key associated with the stored value + * \param value if the key is found, the value is stored in this parameter */ void pop_at_key(uint64_t key, std::unique_ptr & value) @@ -147,8 +154,8 @@ public: * After insertion, if a pair was replaced, then value will contain ownership * of that displaced value. Otherwise it will be a nullptr. * - * /param key the key associated with the value to be stored - * /param value the value to store, and optionally the value displaced + * \param key the key associated with the value to be stored + * \param value the value to store, and optionally the value displaced */ bool push_and_replace(uint64_t key, std::unique_ptr & value) @@ -188,9 +195,11 @@ private: typename std::vector::iterator get_iterator_of_key(uint64_t key) { - auto it = std::find_if(elements_.begin(), elements_.end(), [key] (element & e) -> bool { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + auto it = std::find_if(elements_.begin(), elements_.end(), [key](element & e) -> bool { return e.key == key && e.in_use; }); + // *INDENT-ON* return it; } diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index a4d30a8..03ff9de 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -200,7 +200,7 @@ public: private: RCLCPP_DISABLE_COPY(Node); - static const rosidl_message_type_support_t * ipm_ts; + static const rosidl_message_type_support_t * ipm_ts_; bool group_in_node(callback_group::CallbackGroup::SharedPtr & group); @@ -312,10 +312,8 @@ private: } }; -const rosidl_message_type_support_t * Node::ipm_ts = - rosidl_generator_cpp::get_message_type_support_handle< - rcl_interfaces::msg::IntraProcessMessage - >(); +const rosidl_message_type_support_t * Node::ipm_ts_ = + rosidl_generator_cpp::get_message_type_support_handle(); } /* namespace node */ } /* namespace rclcpp */ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index a6a46e5..23cdd27 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -43,9 +43,9 @@ using namespace rclcpp::node; Node::Node(const std::string & node_name, bool use_intra_process_comms) : Node( - node_name, - rclcpp::contexts::default_context::get_global_default_context(), - use_intra_process_comms) + node_name, + rclcpp::contexts::default_context::get_global_default_context(), + use_intra_process_comms) {} Node::Node( @@ -87,6 +87,7 @@ Node::Node( // *INDENT-ON* } // Initialize node handle shared_ptr with custom deleter. + // *INDENT-OFF* node_handle_.reset(node, [](rmw_node_t * node) { auto ret = rmw_destroy_node(node); if (ret != RMW_RET_OK) { @@ -94,6 +95,7 @@ Node::Node( stderr, "Error in destruction of rmw node handle: %s\n", rmw_get_error_string_safe()); } }); + // *INDENT-ON* using rclcpp::callback_group::CallbackGroupType; default_callback_group_ = create_callback_group( @@ -135,7 +137,7 @@ Node::create_publisher( if (use_intra_process_comms_) { rmw_publisher_t * intra_process_publisher_handle = rmw_create_publisher( - node_handle_.get(), ipm_ts, (topic_name + "__intra").c_str(), qos_profile); + node_handle_.get(), ipm_ts_, (topic_name + "__intra").c_str(), qos_profile); if (!intra_process_publisher_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( @@ -149,8 +151,9 @@ Node::create_publisher( uint64_t intra_process_publisher_id = intra_process_manager->add_publisher(publisher); rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager; + // *INDENT-OFF* auto shared_publish_callback = - [weak_ipm] (uint64_t publisher_id, std::shared_ptr msg) -> uint64_t + [weak_ipm](uint64_t publisher_id, std::shared_ptr msg) -> uint64_t { auto ipm = weak_ipm.lock(); if (!ipm) { @@ -163,6 +166,7 @@ Node::create_publisher( uint64_t message_seq = ipm->store_intra_process_message(publisher_id, unique_msg); return message_seq; }; + // *INDENT-ON* publisher->setup_intra_process( intra_process_publisher_id, shared_publish_callback, @@ -225,7 +229,7 @@ Node::create_subscription( // Setup intra process. if (use_intra_process_comms_) { rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription( - node_handle_.get(), ipm_ts, + node_handle_.get(), ipm_ts_, (topic_name + "__intra").c_str(), qos_profile, false); if (!subscriber_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) @@ -238,10 +242,11 @@ Node::create_subscription( rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager; uint64_t intra_process_subscription_id = intra_process_manager->add_subscription(sub_base_ptr); + // *INDENT-OFF* sub->setup_intra_process( intra_process_subscription_id, intra_process_subscriber_handle, - [weak_ipm] ( + [weak_ipm]( uint64_t publisher_id, uint64_t message_sequence, uint64_t subscription_id, @@ -255,6 +260,7 @@ Node::create_subscription( } ipm->take_intra_process_message(publisher_id, message_sequence, subscription_id, message); }); + // *INDENT-ON* } // Assign to a group. if (group) { diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index ad9102b..6f0ae14 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -42,6 +42,7 @@ namespace publisher class Publisher { friend rclcpp::node::Node; + public: RCLCPP_SMART_PTR_DEFINITIONS(Publisher); @@ -107,7 +108,7 @@ public: } } - std::string + const std::string & get_topic_name() const { return topic_; @@ -119,7 +120,8 @@ public: return queue_size_; } - typedef std::function)> StoreSharedMessageCallbackT; + typedef std::function)> StoreSharedMessageCallbackT; + protected: void setup_intra_process( diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 13235c3..33dc9ff 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -185,9 +185,9 @@ public: private: typedef - std::function< - void (uint64_t, uint64_t, uint64_t, std::unique_ptr &) - > GetMessageCallbackType; + std::function< + void (uint64_t, uint64_t, uint64_t, std::unique_ptr &) + > GetMessageCallbackType; void setup_intra_process( uint64_t intra_process_subscription_id, diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index 29bc717..8a57705 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -12,46 +12,76 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include #include // Mock up publisher and subscription base to avoid needing an rmw impl. -namespace rclcpp { namespace publisher { namespace mock { +namespace rclcpp +{ +namespace publisher +{ +namespace mock +{ class Publisher { public: RCLCPP_SMART_PTR_DEFINITIONS(Publisher); - Publisher() : mock_topic_name(""), mock_queue_size(0) {} + Publisher() + : mock_topic_name(""), mock_queue_size(0) {} - std::string get_topic_name() const {return mock_topic_name;} - size_t get_queue_size() const {return mock_queue_size;} + const std::string & get_topic_name() const + { + return mock_topic_name; + } + size_t get_queue_size() const + { + return mock_queue_size; + } std::string mock_topic_name; size_t mock_queue_size; }; -}}} +} +} +} -namespace rclcpp { namespace subscription { namespace mock { +namespace rclcpp +{ +namespace subscription +{ +namespace mock +{ class SubscriptionBase { public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionBase); - SubscriptionBase() : mock_topic_name(""), mock_queue_size(0) {} + SubscriptionBase() + : mock_topic_name(""), mock_queue_size(0) {} - std::string get_topic_name() const {return mock_topic_name;} - size_t get_queue_size() const {return mock_queue_size;} + const std::string & get_topic_name() const + { + return mock_topic_name; + } + size_t get_queue_size() const + { + return mock_queue_size; + } std::string mock_topic_name; size_t mock_queue_size; }; -}}} +} +} +} // Prevent rclcpp/publisher.hpp and rclcpp/subscription.hpp from being imported. #define RCLCPP_RCLCPP_PUBLISHER_HPP_ @@ -66,16 +96,16 @@ public: #include /* -This tests the "normal" usage of the class: - - Creates two publishers on different topics. - - Creates a subscription which matches one of them. - - Publishes on each publisher with different message content. - - Try's to take the message from the non-matching publish, should fail. - - Try's to take the message from the matching publish, should work. - - Asserts the message it got back was the one that went in (since there's only one subscription). - - Try's to take the message again, should fail. -*/ -TEST(test_intra_process_manager, nominal) { + This tests the "normal" usage of the class: + - Creates two publishers on different topics. + - Creates a subscription which matches one of them. + - Publishes on each publisher with different message content. + - Try's to take the message from the non-matching publish, should fail. + - Try's to take the message from the matching publish, should work. + - Asserts the message it got back was the one that went in (since there's only one subscription). + - Try's to take the message again, should fail. + */ +TEST(TestIntraProcessManager, nominal) { rclcpp::intra_process_manager::IntraProcessManager ipm; auto p1 = std::make_shared(); @@ -97,8 +127,9 @@ TEST(test_intra_process_manager, nominal) { auto ipm_msg = std::make_shared(); ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) + ); auto p1_m1_original_address = unique_msg.get(); auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); @@ -153,13 +184,13 @@ TEST(test_intra_process_manager, nominal) { } /* -Simulates the case where a publisher is removed between publishing and the matching take. - - Creates a publisher and subscription on the same topic. - - Publishes a message. - - Remove the publisher. - - Try's to take the message, should fail since the publisher (and its storage) is gone. -*/ -TEST(test_intra_process_manager, remove_publisher_before_trying_to_take) { + Simulates the case where a publisher is removed between publishing and the matching take. + - Creates a publisher and subscription on the same topic. + - Publishes a message. + - Remove the publisher. + - Try's to take the message, should fail since the publisher (and its storage) is gone. + */ +TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) { rclcpp::intra_process_manager::IntraProcessManager ipm; auto p1 = std::make_shared(); @@ -176,8 +207,9 @@ TEST(test_intra_process_manager, remove_publisher_before_trying_to_take) { auto ipm_msg = std::make_shared(); ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) + ); auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); ASSERT_EQ(nullptr, unique_msg); @@ -189,15 +221,15 @@ TEST(test_intra_process_manager, remove_publisher_before_trying_to_take) { } /* -Tests whether or not removed subscriptions affect take behavior. - - Creates a publisher and three subscriptions on the same topic. - - Publish a message, keep the original point for later comparison. - - Take with one subscription, should work. - - Remove a different subscription. - - Take with the final subscription, should work. - - Assert the previous take returned ownership of the original object published. -*/ -TEST(test_intra_process_manager, removed_subscription_affects_take) { + Tests whether or not removed subscriptions affect take behavior. + - Creates a publisher and three subscriptions on the same topic. + - Publish a message, keep the original point for later comparison. + - Take with one subscription, should work. + - Remove a different subscription. + - Take with the final subscription, should work. + - Assert the previous take returned ownership of the original object published. + */ +TEST(TestIntraProcessManager, removed_subscription_affects_take) { rclcpp::intra_process_manager::IntraProcessManager ipm; auto p1 = std::make_shared(); @@ -224,8 +256,9 @@ TEST(test_intra_process_manager, removed_subscription_affects_take) { auto ipm_msg = std::make_shared(); ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) + ); auto original_message_pointer = unique_msg.get(); auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); @@ -259,12 +292,12 @@ TEST(test_intra_process_manager, removed_subscription_affects_take) { } /* -This tests normal operation with multiple subscriptions and one publisher. - - Creates a publisher and three subscriptions on the same topic. - - Publish a message. - - Take with each subscription, checking that the last takes the original back. -*/ -TEST(test_intra_process_manager, multiple_subscriptions_one_publisher) { + This tests normal operation with multiple subscriptions and one publisher. + - Creates a publisher and three subscriptions on the same topic. + - Publish a message. + - Take with each subscription, checking that the last takes the original back. + */ +TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) { rclcpp::intra_process_manager::IntraProcessManager ipm; auto p1 = std::make_shared(); @@ -291,8 +324,9 @@ TEST(test_intra_process_manager, multiple_subscriptions_one_publisher) { auto ipm_msg = std::make_shared(); ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) + ); auto original_message_pointer = unique_msg.get(); auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); @@ -327,12 +361,12 @@ TEST(test_intra_process_manager, multiple_subscriptions_one_publisher) { } /* -This tests normal operation with multiple publishers and one subscription. - - Creates a publisher and three subscriptions on the same topic. - - Publish a message. - - Take with each subscription, checking that the last takes the original back. -*/ -TEST(test_intra_process_manager, multiple_publishers_one_subscription) { + This tests normal operation with multiple publishers and one subscription. + - Creates a publisher and three subscriptions on the same topic. + - Publish a message. + - Take with each subscription, checking that the last takes the original back. + */ +TEST(TestIntraProcessManager, multiple_publishers_one_subscription) { rclcpp::intra_process_manager::IntraProcessManager ipm; auto p1 = std::make_shared(); @@ -360,8 +394,9 @@ TEST(test_intra_process_manager, multiple_publishers_one_subscription) { // First publish ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) + ); auto original_message_pointer1 = unique_msg.get(); auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); @@ -417,12 +452,12 @@ TEST(test_intra_process_manager, multiple_publishers_one_subscription) { } /* -This tests normal operation with multiple publishers and subscriptions. - - Creates three publishers and three subscriptions on the same topic. - - Publish a message on each publisher. - - Take from each publisher with each subscription, checking the pointer. -*/ -TEST(test_intra_process_manager, multiple_publishers_multiple_subscription) { + This tests normal operation with multiple publishers and subscriptions. + - Creates three publishers and three subscriptions on the same topic. + - Publish a message on each publisher. + - Take from each publisher with each subscription, checking the pointer. + */ +TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) { rclcpp::intra_process_manager::IntraProcessManager ipm; auto p1 = std::make_shared(); @@ -460,8 +495,9 @@ TEST(test_intra_process_manager, multiple_publishers_multiple_subscription) { // First publish ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) + ); auto original_message_pointer1 = unique_msg.get(); auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); @@ -571,15 +607,15 @@ TEST(test_intra_process_manager, multiple_publishers_multiple_subscription) { } /* -Tests displacing a message from the ring buffer before take is called. - - Creates a publisher (buffer_size = 2) and a subscription on the same topic. - - Publish a message on the publisher. - - Publish another message. - - Take the second message. - - Publish a message. - - Try to take the first message, should fail. -*/ -TEST(test_intra_process_manager, ring_buffer_displacement) { + Tests displacing a message from the ring buffer before take is called. + - Creates a publisher (buffer_size = 2) and a subscription on the same topic. + - Publish a message on the publisher. + - Publish another message. + - Take the second message. + - Publish a message. + - Try to take the first message, should fail. + */ +TEST(TestIntraProcessManager, ring_buffer_displacement) { rclcpp::intra_process_manager::IntraProcessManager ipm; auto p1 = std::make_shared(); @@ -596,8 +632,9 @@ TEST(test_intra_process_manager, ring_buffer_displacement) { auto ipm_msg = std::make_shared(); ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) + ); auto original_message_pointer1 = unique_msg.get(); auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); @@ -640,13 +677,13 @@ TEST(test_intra_process_manager, ring_buffer_displacement) { } /* -Simulates race condition where a subscription is created after publish. - - Creates a publisher. - - Publish a message on the publisher. - - Create a subscription on the same topic. - - Try to take the message with the newly created subscription, should fail. -*/ -TEST(test_intra_process_manager, subscription_creation_race_condition) { + Simulates race condition where a subscription is created after publish. + - Creates a publisher. + - Publish a message on the publisher. + - Create a subscription on the same topic. + - Try to take the message with the newly created subscription, should fail. + */ +TEST(TestIntraProcessManager, subscription_creation_race_condition) { rclcpp::intra_process_manager::IntraProcessManager ipm; auto p1 = std::make_shared(); @@ -658,8 +695,9 @@ TEST(test_intra_process_manager, subscription_creation_race_condition) { auto ipm_msg = std::make_shared(); ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) + ); auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); ASSERT_EQ(nullptr, unique_msg); @@ -675,14 +713,14 @@ TEST(test_intra_process_manager, subscription_creation_race_condition) { } /* -Simulates race condition where a publisher goes out of scope before take. - - Create a subscription. - - Creates a publisher on the same topic in a scope. - - Publish a message on the publisher in a scope. - - Let the scope expire. - - Try to take the message with the subscription, should fail. -*/ -TEST(test_intra_process_manager, publisher_out_of_scope_take) { + Simulates race condition where a publisher goes out of scope before take. + - Create a subscription. + - Creates a publisher on the same topic in a scope. + - Publish a message on the publisher in a scope. + - Let the scope expire. + - Try to take the message with the subscription, should fail. + */ +TEST(TestIntraProcessManager, publisher_out_of_scope_take) { rclcpp::intra_process_manager::IntraProcessManager ipm; auto s1 = std::make_shared(); @@ -703,8 +741,9 @@ TEST(test_intra_process_manager, publisher_out_of_scope_take) { auto ipm_msg = std::make_shared(); ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) + ); p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); ASSERT_EQ(nullptr, unique_msg); @@ -720,12 +759,12 @@ TEST(test_intra_process_manager, publisher_out_of_scope_take) { } /* -Simulates race condition where a publisher goes out of scope before store. - - Creates a publisher in a scope. - - Let the scope expire. - - Publish a message on the publisher in a scope, should throw. -*/ -TEST(test_intra_process_manager, publisher_out_of_scope_store) { + Simulates race condition where a publisher goes out of scope before store. + - Creates a publisher in a scope. + - Let the scope expire. + - Publish a message on the publisher in a scope, should throw. + */ +TEST(TestIntraProcessManager, publisher_out_of_scope_store) { rclcpp::intra_process_manager::IntraProcessManager ipm; uint64_t p1_id; @@ -740,8 +779,9 @@ TEST(test_intra_process_manager, publisher_out_of_scope_store) { auto ipm_msg = std::make_shared(); ipm_msg->message_sequence = 42; ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( + new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) + ); EXPECT_THROW(ipm.store_intra_process_message(p1_id, unique_msg), std::runtime_error); ASSERT_EQ(nullptr, unique_msg); diff --git a/rclcpp/test/test_mapped_ring_buffer.cpp b/rclcpp/test/test_mapped_ring_buffer.cpp index 4e40882..3cb6128 100644 --- a/rclcpp/test/test_mapped_ring_buffer.cpp +++ b/rclcpp/test/test_mapped_ring_buffer.cpp @@ -17,9 +17,9 @@ #include /* -Tests get_copy and pop on an empty mrb. -*/ -TEST(test_mapped_ring_buffer, empty) { + Tests get_copy and pop on an empty mrb. + */ +TEST(TestMappedRingBuffer, empty) { // Cannot create a buffer of size zero. EXPECT_THROW(rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(0), std::invalid_argument); // Getting or popping an empty buffer should result in a nullptr. @@ -34,9 +34,9 @@ TEST(test_mapped_ring_buffer, empty) { } /* -Tests push_and_replace with a temporary object. -*/ -TEST(test_mapped_ring_buffer, temporary_l_value) { + Tests push_and_replace with a temporary object. + */ +TEST(TestMappedRingBuffer, temporary_l_value) { rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); // Pass in value with temporary object mrb.push_and_replace(1, std::unique_ptr(new char('a'))); @@ -53,9 +53,9 @@ TEST(test_mapped_ring_buffer, temporary_l_value) { } /* -Tests normal usage of the mrb. -*/ -TEST(test_mapped_ring_buffer, nominal) { + Tests normal usage of the mrb. + */ +TEST(TestMappedRingBuffer, nominal) { rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); std::unique_ptr expected(new char('a')); // Store expected value's address for later comparison. @@ -66,12 +66,16 @@ TEST(test_mapped_ring_buffer, nominal) { std::unique_ptr actual; mrb.get_copy_at_key(1, actual); EXPECT_NE(nullptr, actual); - if (actual) EXPECT_EQ('a', *actual); + if (actual) { + EXPECT_EQ('a', *actual); + } EXPECT_NE(expected_orig, actual.get()); mrb.pop_at_key(1, actual); EXPECT_NE(nullptr, actual); - if (actual) EXPECT_EQ('a', *actual); + if (actual) { + EXPECT_EQ('a', *actual); + } EXPECT_EQ(expected_orig, actual.get()); mrb.get_copy_at_key(1, actual); @@ -91,17 +95,21 @@ TEST(test_mapped_ring_buffer, nominal) { mrb.get_copy_at_key(2, actual); EXPECT_NE(nullptr, actual); - if (actual) EXPECT_EQ('b', *actual); + if (actual) { + EXPECT_EQ('b', *actual); + } mrb.get_copy_at_key(3, actual); EXPECT_NE(nullptr, actual); - if (actual) EXPECT_EQ('c', *actual); + if (actual) { + EXPECT_EQ('c', *actual); + } } /* -Tests get_ownership on a normal mrb. -*/ -TEST(test_mapped_ring_buffer, get_ownership) { + Tests get_ownership on a normal mrb. + */ +TEST(TestMappedRingBuffer, get_ownership) { rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); std::unique_ptr expected(new char('a')); // Store expected value's address for later comparison. @@ -112,17 +120,23 @@ TEST(test_mapped_ring_buffer, get_ownership) { std::unique_ptr actual; mrb.get_copy_at_key(1, actual); EXPECT_NE(nullptr, actual); - if (actual) EXPECT_EQ('a', *actual); + if (actual) { + EXPECT_EQ('a', *actual); + } EXPECT_NE(expected_orig, actual.get()); mrb.get_ownership_at_key(1, actual); EXPECT_NE(nullptr, actual); - if (actual) EXPECT_EQ('a', *actual); + if (actual) { + EXPECT_EQ('a', *actual); + } EXPECT_EQ(expected_orig, actual.get()); mrb.pop_at_key(1, actual); EXPECT_NE(nullptr, actual); - if (actual) EXPECT_EQ('a', *actual); // The value should be the same. + if (actual) { + EXPECT_EQ('a', *actual); // The value should be the same. + } EXPECT_NE(expected_orig, actual.get()); // Even though we pop'ed, we didn't get the original. mrb.get_copy_at_key(1, actual); @@ -130,9 +144,9 @@ TEST(test_mapped_ring_buffer, get_ownership) { } /* -Tests the affect of reusing keys (non-unique keys) in a mrb. -*/ -TEST(test_mapped_ring_buffer, non_unique_keys) { + Tests the affect of reusing keys (non-unique keys) in a mrb. + */ +TEST(TestMappedRingBuffer, non_unique_keys) { rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); std::unique_ptr input(new char('a')); @@ -145,10 +159,14 @@ TEST(test_mapped_ring_buffer, non_unique_keys) { std::unique_ptr actual; mrb.pop_at_key(1, actual); EXPECT_NE(nullptr, actual); - if (actual) EXPECT_EQ('a', *actual); + if (actual) { + EXPECT_EQ('a', *actual); + } actual = nullptr; mrb.pop_at_key(1, actual); EXPECT_NE(nullptr, actual); - if (actual) EXPECT_EQ('b', *actual); + if (actual) { + EXPECT_EQ('b', *actual); + } }